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ABSTRACT 


This  thesis  presents  new  techniques  for  planning  and  robustly  controlling  the  motion 
of  nonlinear  underactuated  vehicles  when  disturbances  are  present  and  only  imperfect 
state  measurements  are  available  for  feedback.  The  basic  motion  planning  algorithm  uses 
cubic  splines  or  Pythagorean  hodograph  curves  to  connect  the  initial  and  final  configura¬ 
tions  and  to  generate  a  feasible  trajectory  for  the  system.  The  feasible  trajectory  and  its 
control  inputs  are  improved  through  an  iterative  i/°°-filter.  Techniques  are  demonstrated 
for  generalizing  the  motion  planning  algorithm  to  address  the  obstacle  avoidance,  multi¬ 
ple  vehicle,  and  minimum  distance  planning  problems.  To  track  a  desired  trajectory,  first 
a  state  feedback  control  law  is  developed  for  the  linearized  system  using  an  i7°°-optimal 
design.  The  state  feedback  controller  produces  a  locally  exponentially  stable  closed-loop 
system  and  guarantees  a  precomputable  level  of  disturbance  attenuation  for  the  system. 
Subsequently,  an  imperfect  state  measurement  feedback  controller  is  developed  by  com¬ 
bining  a  state  estimate  with  the  state  feedback  control  law.  The  state  estimator  exploits 
a  unique  structure  in  the  nonlinear  equations  of  motion  to  decompose  the  system  into 
two  interlaced  subsystems,  which  leads  to  a  direct  solution.  The  estimator  is  actually 
an  i?°°-filter,  and  under  it  the  controller  achieves  a  modified  form  of  disturbance  at¬ 
tenuation.  Simulations  included  in  the  thesis  illustrate  the  motion  planning,  the  state 
feedback  control,  and  the  imperfect  state  measurement  control  algorithms  derived,  for 
selected  models  of  nonlinear  underactuated  vehicles. 
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CHAPTER  1 


INTRODUCTION 


The  primary  objective  of  this  thesis  research  has  been  to  undertake  a  comprehensive 
study  of  motion  planning  and  tracking  control  issues  that  arise  in  the  context  of  non¬ 
linear  underactuated  vehicles.  Motion  planning  involves  the  development  of  a  feasible 
trajectory  by  fully  accounting  for  the  underactuated  structure  of  the  vehicle.  Tracking 
control  involves  the  design  of  controllers  that  use  noisy  partial  state  measurements  to 
generate  inputs  that  make  the  vehicle  track  the  feasible  trajectory.  Usually,  underac¬ 
tuated  vehicles  require  manual  planning  and  a  skilled  operator  to  perform  even  basic 
maneuvers.  Automating  these  tasks  will  allow  us  to  sever  the  link  between  the  vehicle 
and  the  operator,  which  can  lead  to  improved  performance  and  expand  the  range  of 
applications  for  the  vehicle.  Before  we  can  cut  this  vehicle-operator  link,  however,  we 
must  overcome  the  significant  design  challenges  that  arise  when  we  merge  nonlinearities 
and  under  actuation  into  a  single  problem.  The  well  established  techniques  for  planning 
motions  and  controlling  linear  fully  actuated  systems  are  inadequate  for  nonlinear  under¬ 
actuated  vehicles.  Solving  this  design  problem  requires  us  to  fully  understand  the  limits 
of  the  current  techniques  and  then  advance  the  theory  to  develop  practical  solutions. 
This  study  has  addressed  this  challenge,  and  obtained  novel  motion  planning  algorithms 
and  feedback  tracking  control  laws  using  the  framework  and  tools  of  #°°  control  and 
filtering.  When  combined,  these  new  algorithms  allow  us  to  guide  the  underactuated 
vehicle  between  two  configurations  during  a  specified  time  interval.  The  H°°  techniques 
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provide  a  way  to  directly  calculate  practical  solutions  that  are  robust  to  the  disturbances 
the  vehicle  will  likely  encounter. 

This  first  chapter  will  present  a  detailed  description  of  the  basic  problem  we  will  be 
addressing  and  provide  motivation  for  considering  this  area  of  study.  We  will  also  describe 
the  original  contributions  we  have  made  with  this  research.  The  chapter  concludes  with 
a  description  of  the  model  of  an  underactuated  vehicle  that  we  will  use  to  demonstrate 
the  new  motion  planning  and  control  techniques. 


1.1  Problem  Description 

The  vehicles  of  interest  in  this  project  display  four  attributes  that  make  the  motion 
planning  and  control  problems  challenging.  The  attributes  are  that  (i)  the  vehicles  are 
underactuated,  (ii)  the  equations  of  motion  are  nonlinear,  (iii)  there  are  unknown  distur¬ 
bances  entering  the  equations  of  motion,  and  (iv)  the  state  of  the  system  is  only  available 
through  noisy  partial  state  measurements.  Individually,  these  factors  can  be  difficult  to 
handle,  so  their  combination  in  a  single  design  problem  presents  a  formidable  task.  Each 
of  these  design  challenges  merits  a  brief  explanation. 

An  underactuated  system  is  defined  to  be  one  where  the  dimension  of  the  space 
spanned  by  the  control  vector  is  less  than  the  dimension  of  the  configuration  space  [1]. 
Simply  stated,  this  refers  to  a  mechanical  system  that  has  fewer  control  inputs  than 
degrees  of  freedom.  An  equivalent  characterization  of  an  underactuated  system  is  that 
it  has  nonintegrable  acceleration  relations  or  dynamics  [2].  Another  common  description 
for  an  underactuated  system  is  that  it  has  second-order  nonholonomic  constraints.  First- 
order  nonholonomic  constraints  refer  to  restrictions  on  the  velocities  of  the  system,  while 
second-order  nonholonomic  constraints  restrict  the  accelerations  of  the  system.  The  ex¬ 
ample  underactuated  vehicle  we  will  focus  on  is  a  ship  moving  on  a  planar  surface  that 
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has  a  forward  acceleration  input  from  the  propeller  thrust  and  a  pure  angular  acceleration 
input  from  the  rudder.  There  is  no  lateral  acceleration  input  for  the  ship,  which  makes 
the  sideways  motion  the  underactuated  dimension.  Other  examples  of  underactuated 
vehicles  include  spacecraft,  battle-damaged  aircraft,  hovercraft,  and  missiles. 

In  addition  to  being  underactuated,  the  vehicles  studied  in  this  thesis  will  have  non¬ 
linear  equations  of  motion.  Nonlinear  systems  present  their  own  set  of  design  challenges 
if  we  want  to  plan  a  set  of  motions  and  control  the  vehicle  to  follow  a  specific  trajectory. 
The  standard  tools  used  to  control  nonlinear  systems,  including  feedback  linearization 
and  integrator  backstepping,  do  not  provide  direct  solutions  for  the  underactuated  sys¬ 
tems  we  will  consider,  so  new  techniques  must  be  developed.  The  new  techniques  to  be 
presented  in  this  thesis  involve  H°°- optimal  control  design  methods  and  will  exploit  a 
unique  decomposition  of  the  equations  of  motion  that  greatly  simplifies  the  analysis. 

The  third  factor  in  the  problem  that  contributes  to  the  design  challenge  is  the  presence 
of  disturbances  in  the  equations  of  motion.  For  the  control  design  we  will  adopt  a 
game-theoretic  approach  to  solve  the  JT°°-optimal  control  problem.  Accordingly,  we  will 
consider  the  disturbances  as  an  input  to  the  system  selected  by  an  adversary  who  is  trying 
to  disrupt  the  motion  of  vehicle  as  much  as  possible.  Our  objective  will  be  to  develop 
a  feedback  control  law  that  minimizes  the  maximum  negative  effect  the  adversary  can 
have.  The  result  will  be  a  control  law  that  attenuates  the  effect  of  the  disturbances  on  the 
motion  of  the  vehicle  and  allows  it  to  track  a  desired  trajectory.  This  minimax  approach 
to  control  design  has  documented  solutions  for  both  linear  and  nonlinear  systems.  The 
solution  for  nonlinear  systems  is  expressed  in  terms  of  a  partial  differential  equation, 
which,  in  general,  does  not  admit  a  closed-form  solution.  To  develop  a  closed-form 
solution,  we  will  linearize  the  vehicle’s  equations  of  motion  about  a  desired  trajectory  to 
find  the  optimal  control  inputs. 
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The  final  design  challenge  in  this  thesis  is  the  imperfect  state  measurement  restriction 
on  the  control  law.  In  this  case,  imperfect  state  measurement  means  that  we  can  measure 
only  some  of  the  states  in  the  system  and  there  are  unknown  disturbances  entering  the 
measurements.  The  solution  to  the  Ff°°-optimal  control  problem  for  the  imperfect  state 
measurement  case  is  well  known  and  can  be  computed  directly  for  linear  systems.  For 
nonlinear  systems,  the  solution  involves  two  partial  differential  equations,  which  usually 
cannot  be  solved  explicitly.  Our  approach  will  use  the  structure  of  the  underactuated 
vehicle  model  to  reduce  the  nonlinear  problem  to  a  pair  of  simpler  subproblems.  We  will 
demonstrate  that  we  can  build  on  the  established  results  for  linear  systems  to  achieve  a 
form  of  disturbance  attenuation  for  the  nonlinear  underactuated  vehicle. 

With  the  four  primary  elements  of  the  system  in  place,  we  can  now  make  a  formal 
statement  of  the  problems  this  research  effort  will  address.  We  assume  we  are  given  an 
accurate,  time-invariant  model  of  an  underactuated  vehicle  that  we  want  to  control.  In 
addition,  we  are  given  the  initial  and  final  configurations  between  which  we  want  to  move 
the  vehicle,  and  a  time  interval  for  completing  the  task.  Our  first  objective  is  to  develop  a 
motion  planning  algorithm  to  design  a  feasible  trajectory  for  the  underactuated  vehicle  to 
move  from  one  configuration  to  another  during  the  specified  time  interval.  The  outcome 
of  the  motion  planning  algorithm  will  be  a  set  of  time-varying  control  inputs  that  can 
be  applied  to  a  model  of  the  underactuated  vehicle  to  generate  a  desired  trajectory. 
The  planning  algorithm  will  focus  on  finding  a  direct  method  for  computing  the  control 
inputs  as  opposed  to  an  exhaustive  search  technique.  The  second  objective  is  to  create 
a  controller  design  technique  that  causes  an  underactuated  vehicle  to  track  the  desired 
trajectory  while  also  attenuating  the  effect  of  disturbances.  The  control  law  will  be 
restricted  to  use  only  imperfect  state  measurements.  The  control  design  problem  can  be 
subdivided  into  two  parts.  The  first  part  will  be  to  design  a  state  feedback  controller 
for  the  nonlinear  under  actuated  system  assuming  perfect  state  information  is  available 
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to  the  controller.  The  second  part  will  be  to  modify  the  state  feedback  controller  to 
account  for  the  imperfect  state  measurement  restriction.  For  both  the  motion  planning 
and  the  control  aspects  of  the  problem,  we  want  to  exploit  the  known  dynamics  of  the 
underactuated  vehicle  to  develop  natural  motions  for  the  system  to  achieve  the  objectives. 

We  note  that  the  motion  planning  and  control  aspects  of  the  problem  can  be  com¬ 
pletely  decoupled  and  addressed  separately,  but  we  will  attempt  to  plan  motions  that 
make  the  tracking  control  problem  easier  to  solve.  We  also  observe  that  the  motion  plan¬ 
ning  algorithm  yields  a  feasible  desired  trajectory  that  the  underactuated  vehicle  could 
track  if  it  had  the  proper  initial  conditions  and  there  were  no  disturbances  present.  The 
control  law  developed  here  is  valuable  because  it  will  recover  from  initial  condition  errors 
and  successfully  track  the  desired  trajectory  even  in  the  presence  of  disturbances. 

To  formulate  a  meaningful  and  realistic  thesis  research  project,  there  should  be  lim¬ 
itations  on  the  scope  of  the  research  to  make  the  problem  manageable.  We  used  the 
following  limitations  as  guidelines  to  provide  a  structure  for  this  research.  To  keep  the 
research  focused,  we  did  not  address  three  areas  of  control  theory  that  may  be  relevant  to 
the  problem.  First,  we  did  not  use  adaptive  methods  as  a  primary  technique  for  vehicle 
motion  control.  Adaptive  control  offers  advantages  when  the  true  nature  of  the  system  is 
unknown  or  is  changing  over  time  and  system  parameters  need  to  be  identified.  Our  re¬ 
search  assumed  we  had  a  well-defined  vehicle  moving  with  known  properties.  The  second 
area  of  research  we  excluded  was  biologically  inspired  controller  design,  which  includes 
neural  networks,  fuzzy  logic,  and  genetic  algorithms.  These  methods  offer  valid  controller 
design  techniques  which  are  suitable  for  some  nonlinear  systems,  but  we  avoided  these 
approaches  because  they  do  not  always  offer  good  insight  into  the  nature  of  the  controller 
and  make  it  difficult  to  precisely  describe  the  contributions  to  control  theory.  Finally, 
the  research  did  not  focus  on  the  stochastic  nature  of  the  problem.  There  are  certainly 
many  aspects  of  vehicle  motion  control  that  could  be  characterized  in  probabilistic  terms, 
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but  we  will  not  explore  this  option.  These  three  approaches  may  offer  good  results  in 
practice,  so  they  may  be  viable  directions  for  future  research  projects.  These  restrictions 
to  the  scope  of  the  project  were  relatively  mild  and  still  offered  ample  room  for  significant 
contributions  to  motion  planning  and  control  for  underactuated  systems. 

This  section  has  outlined  the  four  challenging  aspects  of  the  underactuated  vehicle 
control  problem  and  identified  some  research  guidelines  to  keep  the  effort  manageable. 
We  will  now  explain  why  this  area  of  research  is  important. 

1.2  Motivation 

There  are  two  practical  reasons  for  developing  techniques  to  plan  motions  for  and 
to  control  underactuated  systems.  First,  a  fully  actuated  system  requires  more  control 
inputs  than  an  underactuated  system,  which  means  there  will  have  to  be  more  devices 
to  generate  the  necessary  forces.  The  additional  controlling  devices  add  to  the  cost  and 
weight  of  the  system.  Finding  a  way  to  control  an  underactuated  version  of  the  system 
would  eliminate  some  of  the  controlling  devices  and  could  improve  the  overall  performance 
or  reduce  the  cost.  The  second  practical  reason  for  studying  underactuated  vehicles  is 
that  underactuation  provides  a  backup  control  technique  for  a  fully  actuated  system.  If 
a  fully  actuated  system  is  damaged  and  we  have  an  underactuated  controller  available, 
then  we  may  be  able  to  recover  gracefully  from  the  failure.  The  underactuated  controller 
may  be  able  to  salvage  a  system  that  would  otherwise  be  uncontrollable.  This  reason 
for  designing  controllers  for  underactuated  systems  would  be  especially  useful  for  aircraft 
or  spacecraft,  where  actuator  failures  can  be  catastrophic  to  the  vehicle  or  its  mission. 
Section  6.6  will  describe  in  more  detail  how  techniques  for  underactuated  systems  can 
assist  with  failure  recovery  for  fully  actuated  systems. 
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1.3  Contributions 


This  research  effort  has  developed  techniques  for  underactuated  vehicles  and  makes 
contributions  to  both  the  motion  planning  and  the  control  disciplines.  We  will  highlight 
the  key  contributions  presented  in  the  following  chapters. 

There  are  three  significant  results  from  the  motion  planning  research.  First,  we 
developed  a  technique  to  directly  plan  motions  for  planar  underactuated  vehicles  using 
polynomial  curves  and  numerical  methods.  The  polynomial  curves  can  be  cubic  splines, 
Pythagorean  hodograph  curves,  or  other  curves  that  can  be  quickly  generated  to  link 
two  configurations.  We  then  used  the  underactuated  vehicle  model  to  extract  a  complete 
feasible  trajectory  from  the  simple  polynomial  curve.  Section  3.2  provides  more  details 
on  this  contribution.  The  second  major  motion  planning  result  was  to  demonstrate  how 
an  iterative  if°°-filter  could  be  used  to  improve  the  feasible  trajectory  derived  from  the 
polynomial  curve.  The  filter  updates  the  trajectory  and  moves  the  final  configuration 
closer  to  the  goal  configuration  to  improve  the  performance  of  the  motion  planner;  see 
Subsection  3.2.4  for  a  complete  explanation  of  the  iterative  i7°°-filter.  The  final  motion 
planning  result  was  to  show  how  to  incorporate  elements  from  the  basic  motion  planning 
approach  with  established  motion  planning  algorithms  to  solve  more  challenging  planning 
problems  for  an  underactuated  vehicle.  Specifically,  we  identified  the  path  length  of 
the  polynomial  curve  generated  in  the  basic  planning  algorithm  as  a  valuable  part  of  a 
metric  used  in  a  randomized  planning  technique.  We  explain  the  randomized  planning 
technique,  how  to  improve  the  metric  and  how  to  use  the  new  approach  to  address  the 
obstacle  avoidance  and  multiple  vehicle  planning  problems  in  Section  3.3. 

We  would  also  like  to  highlight  two  primary  contributions  from  the  controller  design 
aspects  of  the  problem.  The  first  contribution  was  the  development  of  a  linearized  perfect 
state  feedback  H°°  controller  for  an  underactuated  system.  We  proved  that  the  controller 
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provides  local  exponential  stability  for  the  closed-loop  system  and  offers  a  solution  to  the 
tracking  control  problem.  In  addition,  our  research  characterized  the  disturbance  attenu¬ 
ation  properties  and  the  region  of  attraction  for  the  closed-loop  system  and  demonstrated 
that  the  technique  works  for  both  minimum  and  nonminimum  phase  systems.  Chapter  4 
provides  the  complete  results  for  this  contribution.  The  other  major  result  from  the 
controller  design  research  was  the  development  of  an  JT°°-filter  based  on  a  unique  de¬ 
composition  of  the  underactuated  model  and  used  to  estimate  the  states  of  the  system. 
The  filter  design  is  successful  because  we  can  decompose  the  full  nonlinear  system  into 
two  subsystems,  with  the  property  that  each  is  affine  if  the  state  of  the  other  subsystem 
is  known.  This  structure  allowed  us  to  develop  an  approach  for  estimating  the  complete 
state  from  an  imperfect  partial  state  measurement.  We  then  combined  the  state  estimate 
with  the  state  feedback  controller  to  develop  an  imperfect  state  measurement  controller 
for  the  underactuated  system.  We  were  also  able  to  prove  a  form  of  disturbance  atten¬ 
uation  for  the  closed-loop  system  using  the  imperfect  state  measurement  controller.  We 
present  the  details  behind  this  second  contribution  in  control  design  in  Chapter  5. 

Our  research  has  made  some  additional  contributions  in  other  directions  that  we  will 
describe  as  we  present  the  results  in  Chapters  3,  4,  and  5.  We  highlighted  the  above 
five  contributions  because  they  stand  out  as  the  most  significant  results  of  this  research 
effort. 


1.4  Underactuated  Vehicle  Model 

We  present  in  this  section  the  vehicle  model  used  for  this  research  effort  and  describe 
some  of  its  characteristics.  The  model  was  used  earlier  by  Pettersen  and  Nijmeijer  [3] 
and  we  have  made  only  minor  changes  to  simplify  the  notation.  For  a  more  detailed 
description  of  this  and  similar  models,  see  [4].  The  relevant  equations  of  motion  for  this 
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system  are 


u  =  muvr  —  duu  +  U\  +  W\  (1.1) 

v  =  mvur  —  dvv  +  W2  (1.2) 

r  =  mruv  —  drr  +  U2  +  W3  (1.3) 

x  =  u  cos  (ip)  —  v  sin(-0)  (1.4) 

y  =  u  sm(ip)  +  v  cos(ip)  (1.5) 

ip  —  r  (1.6) 


where  u,  v,  and  r  are  the  body-frame  velocities  in  surge,  sway,  and  yaw,  respectively;  x 
and  y  are  the  inertial  positions;  and  ip  is  the  inertial  rotation  angle.  The  coefficients  m* 
and  di  represent  combined  mass  terms,  including  added  mass,  and  damping  coefficients. 
The  two  control  inputs  are  «i  and  u2.  The  disturbances  only  appear  in  Equations  (1.1) 
through  (1.3)  because  they  represent  forces  or  torques  that  influence  the  acceleration  of 
the  vehicle.  Equations  (1.4)  through  (1.6)  are  just  the  kinematics  of  the  system  relating 
the  inertial  reference  frame  to  the  body  frame,  and  they  do  not  have  any  disturbances. 

One  of  our  research  objectives  is  to  design  the  control  inputs  Ui  and  u2  such  that  the 
vehicle  tracks  a  feasible  trajectory.  A  feasible  trajectory  can  be  generated  by  simulating 
the  ship  model  described  above  without  the  disturbances.  The  following  equations  de¬ 
scribe  the  desired  system,  the  state  of  which  constitutes  a  feasible  trajectory.  In  these 
equations,  the  subscript  d  indicates  the  desired  values  for  the  states  and  control  inputs 

iid  =  mu  vd  rd  -  duud  +  uld 
vd  =  mv  ud  rd  -  dv  vd 
rd  =  mr  ud  vd  —  drrd  +  u2d 
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(1.7) 

(1.8) 
(1.9) 


id 

=  ud  cos  (ipd)  -  Vd  sin(V’d) 

(1.10) 

Vd 

=  ud  sin(V>d)  +  vd  cos  (ipd) 

(l.n) 

ipd 

=  vd 

(1.12) 

where  we  know  the  initial  conditions  of  the  states  and  the  value  of  uu  and  U2d  for  all 
time.  We  note  that  the  motion  planning  algorithm  will  generate  the  control  inputs  uu 
and  u2d  so  that  the  feasible  trajectory  approaches  the  specified  final  configuration. 

An  equivalent  way  to  state  the  tracking  problem  is  to  require  the  difference  between 
the  actual  vehicle  configuration  and  the  desired  configuration  to  approach  zero.  Denote 
the  difference  between  the  two  configurations  as  the  error.  Since  the  vehicle  will  not  nec¬ 
essarily  share  the  same  initial  conditions  as  the  desired  system,  the  tracking  controller 
will  drive  the  error  to  zero  and  minimize  the  effect  of  the  disturbances  as  the  vehicle  con¬ 
verges  to  the  reference  trajectory.  To  help  in  the  analysis,  we  write  the  error  differential 
equations  by  subtracting  the  desired  equations  from  the  system  equations  and  defining 
the  error  variables  as  ae  =  a  —  ad,  where  a  represents  any  state  or  control  variable,  to  get 


«e  =  mu(verd  +  Vdre  +  vere)  -  duue  +  uie  +  wi  (1.13) 

ve  =  mv(ue rd  +  udre  +  ue re)  -  dvve  +  w2  (1.14) 

fe  =  mr(uevd  +  udve  +  ueve)  -  drre  +  u2e  +  w3  (1-15) 

Xe  -  Udlcostyd  +  V’e)  -  COs(V>d)]  +  Ue  COS (V>d  +  V>e) 

-  vd[sm(^d  +  ipe)  -  sin(V’d)]  -  ve  sin(V>d  +  ipe)  (1.16) 

ye  =  ud[sin(V’d  +  A)  ~  sin(^d)]  +  ue  sin(V’d  +  ipe) 

+  Vd[C0S(i>d  +  i>, e )  -  cos(^d)]  +  ve  COS (lpd  +  V’e)  (1-17) 

V’e  =  re.  (1-18) 


We  will  find  it  helpful  to  use  an  alternative,  but  equivalent,  description  for  the  kine¬ 
matic  equations  (1.4)  through  (1.6)  for  parts  of  the  analysis.  The  second  description  uses 
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variables  zi,  Z2,  and  Z3  instead  of  x,  y,  and  ip,  and  the  transformation  is 


Z\  =  x  cos  (ip)  +  y  sin(,0) 

(1.19) 

z2  =  —xsin(ip)  +  ycos(ip) 

(1.20) 

II 

co 

(1.21) 

This  transformation  yields  the  following  three  kinematic  equations  after  taking  time 

derivatives  and  simplifying: 

zi  =  u  +  z2r 

(1.22) 

Z2  =  V  —  Z\V 

(1.23) 

z3  =  r. 

(1.24) 

We  note  that  the  transformation  from  [x,  y,  ip]  to  [z\,  z2, 

given  either  description,  we  can  recover  the  other  one. 

23]  is  invertible,  and  hence 

To  help  ease  notation,  it  will  be  convenient  to  have  a  simple  expression  to  describe 
each  of  the  different  types  of  equations  of  motion.  To  represent  the  actual  system  with 
the  x,  y,  and  ip  variables  in  (1.1)  through  (1.6)  we  use 


q  =  f(t,q)  +  B(t)u  + D(t)w  (1.25) 


where  f(t,  q)  is  a  vector- valued  nonlinear  function  and  the  boldface  variables  represent 
vectors.  Specifically,  we  have 


q 

u 


w 


u  v  r  x  y  ip 
-lT 

U\  U2 


Wi  W2  W3 
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We  also  have  similar  expressions  for  the  desired  system  and  the  error  equations,  given  by 


4*  =  f{t,qd)  +  B(t)ud  (1.26) 

qe  =  fe(t,qe)  +  B(t)ne  + D(t)w.  (1.27) 

Note  that  the  desired  system  equation  given  by  (1.26)  uses  the  same  nonlinear  function 
as  (1.25),  but  the  error  equation  (1.27)  will  have,  in  general,  a  different  nonlinear  function. 
When  we  use  the  Zi  variables  in  the  system  equations,  we  will  include  a  z  subscript  to 
arrive  at  the  following  compact  notation: 

q2  =  /*(i,q»)  +  B(t)uz  +  D{t)  w.  (1.28) 

We  will  introduce  additional  notation  as  required  when  we  analyze  the  problem. 

The  vehicle  model  we  presented  allows  us  to  incorporate  into  the  problem  the  four 
design  challenges  described  in  Section  1.1.  The  system  is  underactuated  because  there 
are  only  two  control  inputs  and  three  degrees  of  freedom  with  the  motion.  The  model  is 
nonlinear  because  it  contains  terms  where  the  states  are  multiplied  together,  as  well  as 
trigonometric  functions.  The  disturbances  appear  explicitly  through  the  wz  terms.  The 
imperfect  state  measurement  feature  arises  naturally  because,  typically,  the  controller 
can  access  only  noisy  measurements  of  the  states  describing  the  position  and  orientation 
of  the  vehicle.  We  will  use  this  model  to  develop  a  solution  to  the  basic  motion  planning 
problem  and  then  expand  that  solution  to  address  the  obstacle  avoidance  and  multiple 
vehicle  planning  problems.  In  addition,  we  will  construct  both  perfect  state  feedback  and 
imperfect  state  measurement  tracking  controllers  based  on  this  model. 

The  remainder  of  this  thesis  is  organized  to  correspond  to  the  sequential  design  process 
for  planning  and  controlling  the  motion  of  an  underactuated  vehicle.  We  start  with  a 
comprehensive  literature  review  in  Chapter  2.  Chapter  3  describes  the  solution  to  the 
basic  motion  planning  problem  and  how  to  combine  the  solution  with  other  planning 


12 


techniques  to  solve  more  challenging  problems.  The  motion  planning  algorithm  generates 
a  feasible  trajectory  for  the  underactuated  vehicle  to  track.  With  a  feasible  trajectory  in 
place,  Chapter  4  solves  the  state  feedback  tracking  control  problem  and  proves  stability 
for  the  closed-loop  system.  We  also  explore  the  disturbance  attenuation  properties  and 
the  region  of  attraction  for  the  state  feedback  controller.  Chapter  5  modifies  the  state 
feedback  controller  to  handle  the  imperfect  state  measurement  case  and  proves  local 
disturbance  attenuation  results.  Chapter  6  provides  simulations  illustrating  the  results  of 
the  motion  planning  and  control  techniques  applied  to  the  underactuated  vehicle  model. 
The  final  chapter  provides  a  summary  of  the  completed  work  and  proposes  additional 
research  areas  that  came  to  light  during  the  investigation. 
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CHAPTER  2 


LITERATURE  REVIEW 

The  literature  associated  with  motion  planning  and  underactuated  vehicle  control 
provides  the  foundation  for  the  current  research  effort.  The  breadth  of  the  existing 
literature  is  quite  large,  so  we  will  present  the  key  works  related  to  the  current  effort. 
Our  original  contributions  build  directly  on  some  of  these  recent  results.  The  review 
below  considers  literature  from  the  motion  planning,  underactuated  vehicle  control,  and 
H°°- optimal  control  disciplines.  We  will  describe  the  literature  from  each  discipline  and 
explain  how  it  relates  to  our  original  contributions. 


2.1  Motion  Planning 

We  will  survey  the  motion  planning  literature  with  two  goals  in  mind.  The  first  goal 
is  to  provide  a  summary  of  the  relevant  research  that  is  related  to  the  motion  planning 
problem  for  a  nonlinear  underactuated  vehicle.  This  summary  will  extract  the  key  ref¬ 
erences  from  the  vast  collection  of  work  on  motion  planning  and  explain  how  they  are 
relevant  to  our  problem.  The  second  goal  for  this  portion  of  the  literature  review  is 
to  justify  the  approach  of  incorporating  randomized  planning  techniques  with  the  basic 
motion  planning  algorithm  to  solve  the  obstacle  avoidance  and  multiple  vehicle  planning 
problems.  There  are  clearly  other  alternatives  for  addressing  these  problems,  but  ran¬ 
domized  techniques  offer  a  fair  balance  between  being  able  to  handle  the  underactuated 
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dynamics  of  the  vehicles  in  a  complex  environment  and  being  able  to  generate  a  solution 
quickly.  Randomized  techniques  do  not  guarantee  that  a  solution  will  be  found,  but  if  a 
solution  is  possible,  it  is  very  likely  they  will  find  it  relatively  quickly. 

2.1.1  General  motion  planning  references 

A  good  starting  point  to  understand  the  motion  planning  problem  and  some  of  the 
common  solution  techniques  is  the  recent  paper  by  Latombe  [5].  This  paper  presents 
a  summary  of  current  motion  planning  research,  describes  some  of  the  new  problems 
that  are  emerging,  and  predicts  what  may  happen  with  the  field  in  the  near  future. 
Latombe  emphasizes  the  fact  that  applications  outside  of  robotics  will  significantly  influ¬ 
ence  motion  planning  research.  In  addition,  he  hints  at  the  fact  that  randomized  planning 
techniques  may  be  a  key  factor  in  developing  solutions  for  systems  with  many  degrees  of 
freedom. 

For  a  more  complete  introduction  to  the  motion  planning  problem,  there  are  four 
valuable  references  that  are  germane  to  our  problem.  The  textbook  by  Latombe  [6]  is 
the  standard  reference  for  basic  motion  planning  concepts  and  provides  a  comprehen¬ 
sive  description  of  the  subject.  Latombe  explains  the  fundamental  techniques,  including 
roadmap,  cell  decomposition,  and  potential  field  planning  methods.  He  also  addresses 
the  more  advanced  problems  of  handling  multiple  objects,  kinematic  constraints,  and 
dealing  with  uncertainty.  Another  resource  is  the  survey  paper  by  Hwang  and  Ahuja  [7], 
which  contains  an  extensive  description  of  the  motion  planning  problem.  The  authors 
also  provide  an  excellent  list  of  references  to  help  guide  additional  research  on  the  sub¬ 
ject.  The  third  general  reference  is  a  collection  edited  by  Li  and  Canny  [8]  which  focuses 
on  nonholonomic  systems.  This  book  offers  information  that  is  more  closely  associated 
with  motion  planning  for  underactuated  vehicles.  The  techniques  addressed  in  the  book 
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include  steering  using  sinusoids  and  differential  geometric  methods.  The  final  reference 
in  this  set  is  the  compilation  edited  by  Laumond  [9].  Laumond’s  book  contains  six  chap¬ 
ters  that  address  different  aspects  of  the  problem,  including  planning  for  nonholonomic 
systems,  probabilistic  methods,  and  collision  detection  algorithms.  Along  with  [5],  these 
four  references  provide  a  thorough  overview  of  the  motion  planning  problem  and  common 
solution  techniques.  However,  to  develop  our  motion  planning  solution,  we  will  have  to 
examine  more  detailed  articles  in  the  literature. 

The  motion  planning  literature  can  be  divided  into  deterministic  and  randomized 
methods.  Deterministic  techniques  offer  direct  approaches  for  developing  solutions  and 
make  it  easier  to  prove  when  a  solution  will  be  found,  but  they  can  be  time  consuming  for 
high-dimensional  problems.  By  their  nature,  the  randomized  methods  make  it  difficult 
to  prove  the  results,  but  they  offer  promising  techniques  that  can  often  solve  challeng¬ 
ing  problems  quickly.  To  help  structure  the  motion  planning  literature  review,  we  will 
first  consider  deterministic  techniques  and  then  shift  our  focus  to  consider  randomized 
planning  methods. 

2.1.2  Deterministic  planning  techniques 

Two  papers  by  Djouani  and  Hamam  [10, 11]  consider  motion  planning  for  a  ship 
and  are  directly  related  to  this  research.  The  authors  form  a  nonlinear  optimal  control 
problem  to  find  a  minimum  time-energy  trajectory.  Their  algorithm  uses  an  augmented 
Lagrangian  approach  to  solve  the  nonlinear  programming  problem,  which  is  similar  to 
a  penalty  function  method.  The  algorithm  produces  the  control  inputs  for  the  ship  to 
approach  the  desired  configuration  and  allows  the  design  to  account  for  obstacles  in  the 
environment.  The  solution  uses  an  iterative  numerical  approach  and  the  authors  do  not 
comment  on  the  computational  requirements  or  how  quickly  the  algorithm  converges. 
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The  overall  approach  is  reasonable  and  offers  an  alternative  to  the  basic  motion  planning 
solution  we  will  present  in  Chapter  3. 

The  motion  planning  problem  we  are  trying  to  solve  can  be  formulated  as  a  two-point 
boundary  value  problem  since  the  initial  and  terminal  conditions  are  known  and  the 
solution  must  satisfy  the  equations  of  motion.  The  multipoint  shooting  method  offers 
one  numerical  approach  to  solve  this  challenging  problem  [12].  With  some  simplification, 
the  method  guesses  a  portion  of  the  solution,  computes  the  trajectory,  uses  the  resulting 
terminal  error  to  update  the  initial  guess,  and  iterates  until  the  final  boundary  condition 
is  satisfied.  Yih  and  Ro  [13]  apply  the  multipoint  shooting  method  to  plan  motions  for 
nonholonomic  systems.  Since  this  is  an  iterative  technique,  our  basic  motion  planning 
algorithm  improves  on  it  by  offering  a  way  to  directly  construct  the  trajectory. 

With  a  slightly  different  formulation,  the  motion  planning  problem  can  also  be  cast  as 
a  nonlinear  optimization  problem  in  an  infinite  dimensional  space.  Fernandes,  Gurvits, 
and  Li  [14, 15]  provide  a  solution  to  this  version  of  the  problem  by  approximating  the 
infinite-dimensional  space  with  a  finite  basis.  The  approximation  reduces  the  task  to 
a  finite  dimensional  problem,  and  the  authors  outline  how  to  construct  the  basis  and 
then  efficiently  solve  the  optimization  problem.  This  approach  is  constructive  and  offers 
another  viable  alternative  to  our  proposed  techniques. 

The  motion  planning  problem  is  posed  as  a  different  type  of  optimal  control  problem 
by  Spangelo  and  Egeland  [16].  The  authors  achieve  optimal  trajectory  planning  and  colli¬ 
sion  avoidance  using  optimal  control  techniques.  Their  innovation  is  to  use  a  performance 
index  that  is  a  combination  of  the  amount  of  energy  and  the  time  required  to  execute  a 
motion.  The  authors  use  path  constraints  to  prevent  collisions.  The  approach  is  based  on 
a  numerical  shooting  method  to  develop  a  solution  and  requires  a  fully  actuated  system 
to  implement  the  results. 
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Arai,  Tanie,  and  Shiroma  collaborated  on  several  motion  planning  articles  that  are 
useful  for  our  problem.  In  [17, 18],  they  use  time-scaling  control  and  a  bidirectional 
motion  planning  approach  to  develop  trajectories  for  a  robot  with  one  passive  joint. 
Time-scaling  allows  the  authors  to  control  the  velocity  of  the  active  joints,  which  in  turn 
provides  control  over  the  drift  of  the  passive  joint.  The  bidirectional  motion  planning 
algorithm  searches  forward  from  the  initial  condition  and  backwards  from  the  terminal 
condition  to  find  a  complete  path.  In  this  work,  the  initial  and  final  configuration  put 
the  robot  at  rest,  which  is  not  necessarily  a  feature  we  want  to  include  for  our  problem. 
In  related  works  [19,20],  the  same  authors  develop  a  motion  planning  algorithm  for  an 
underactuated  robot  that  accounts  for  the  dynamics  of  the  system.  They  construct  a 
trajectory  by  combining  translational  and  rotational  motion  segments  to  achieve  the 
overall  objective.  Although  we  also  exploit  the  dynamics  of  the  vehicle  in  planning  the 
path,  the  results  from  the  work  of  Arai,  Tanie,  and  Shiroma  [17, 18]  apply  to  only  a 
specific  type  of  underactuated  robot  and  do  not  generalize  to  other  systems. 

Lynch,  Shiroma,  Arai,  and  Tanie  [21]  also  use  time-scaling  to  control  an  underactuated 
vehicle.  Their  motion  planning  algorithm  is  a  best-first  search  and  allows  the  authors  to 
avoid  obstacles  in  the  environment.  The  technique  will  yield  a  path  that  approaches  the 
desired  configuration,  but  may  not  reach  it  exactly.  The  search  technique  relies  on  the 
small-time  locally  controllable  property  of  the  robot  to  advance  the  path.  It  does  not 
offer  a  direct  method  for  constructing  a  trajectory. 

Reister  and  Lenhart  [22]  document  a  method  for  finding  time-optimal  paths  for 
constant-speed  vehicles.  The  paths  are  constructed  from  arcs  of  circles  and  straight 
line  segments  to  connect  the  initial  and  final  configurations.  The  approach  contains  el¬ 
ements  similar  to  those  originally  proposed  by  Reeds  and  Shepp  [23].  Reeds  and  Shepp 
catalog  the  types  of  paths  available  to  a  car-like  vehicle  that  can  travel  forwards  and 
backwards  and  describe  an  approach  to  identify  the  shortest  length  path.  Sussmann  and 
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Tang  [24]  advance  the  results  of  Reeds  and  Shepp  by  showing  how  to  construct  the  solu¬ 
tion  using  geometric  techniques  and  nonlinear  optimal  control.  In  addition  to  providing 
an  alternative  derivation  for  the  planning  results,  Sussmann  and  Tang  [24]  were  able  to 
reduce  the  catalog  of  required  paths  to  46  from  the  original  48  proposed  by  Reeds  and 
Shepp.  Soueres  and  Laumond  [25]  further  refined  the  planning  approach  using  Reeds- 
Shepp  curves  by  showing  how  to  partition  the  configuration  space  into  regions  where  the 
same  type  of  path  is  optimal  to  reach  the  region.  This  partitioning  provides  an  efficient 
method  for  selecting  the  appropriate  trajectory  based  on  the  vehicle’s  initial  configura¬ 
tion.  We  will  extract  elements  of  these  approaches  when  we  consider  the  problem  of 
constructing  minimum  distance  trajectories  for  an  underactuated  vehicle. 

Another  approach  to  motion  planning  formulates  the  problem  from  a  game-theoretic 
perspective  and  applies  numerical  techniques  to  search  for  an  optimal  path.  LaValle  [26] 
presents  a  general  framework  for  this  approach  and  applies  it  to  several  variations  of 
the  motion  planning  problem,  including  obstacle  avoidance  and  multiple  vehicle  motion 
planning.  LaValle  and  Hutchinson  [27]  discuss  optimal  motion  planning  for  multiple 
robots.  Their  intent  is  to  solve  the  collision  avoidance  problem  and  simultaneously  opti¬ 
mize  a  performance  measure  for  each  robot.  In  addition,  one  of  their  approaches  using 
independent  roadmaps  for  each  robot  offers  a  compromise  between  centralized  and  de¬ 
coupled  motion  planning  for  multiple  robots.  Briefly  stated,  centralized  planning  for 
multiple  robots  concatenates  the  individual  robots  into  a  single  system  with  constraints 
that  prevent  collisions.  The  motion  planning  problem  can  then  be  solved  for  the  com¬ 
bined  system.  In  contrast,  the  decoupled  approach  to  planning  solves  the  problem  for 
each  robot  independently  and  then  locally  resolves  conflicting  motions  as  they  arise.  By 
finding  a  balance  between  these  two  common  techniques,  LaValle  and  Hutchinson  [27]  are 
able  to  consider  a  wider  range  of  motions  and  to  find  optimal  solutions  to  the  problem. 
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A  purely  decoupled  planning  algorithm  that  might  be  useful  for  our  situation  also 
poses  the  planning  problem  as  a  noncooperative  dynamic  game  among  the  vehicles. 
Tomlin,  Pappas,  and  Sastry  [28, 29]  developed  this  approach  in  the  context  of  air  traffic 
management,  but  the  main  principles  could  be  adapted  to  handle  underactuated  vehicles. 
The  basic  idea  with  the  approach  is  to  create  a  protective  bubble  around  each  vehicle 
and  to  replan  motions  if  the  existing  plans  cause  a  vehicle  to  trespass  into  another’s  pro¬ 
tected  zone.  The  approach  assumes  the  vehicles  cannot  communicate  and  hence  cannot 
cooperate  in  replanning  their  motions.  To  handle  this  situation,  each  vehicle  will  assume 
the  other  will  take  the  most  hostile  action  possible  and  will  plan  to  maintain  separation 
regardless  of  the  other  vehicle’s  action.  This  worst-case  perspective  can  be  formulated 
as  a  dynamic  game,  and  solutions  can  be  developed  from  the  resulting  Hamilton- Jacobi 
equations.  The  replanned  motions  involve  making  turns  or  adjusting  speeds  to  avoid 
hazardous  situations.  To  adapt  the  approach  for  the  underactuated  vehicle  problem,  we 
would  have  to  account  for  the  new  dynamic  equations  and  introduce  obstacles  into  the 
environment. 

Another  decoupled  approach  treats  the  individual  position  paths  as  deformable  elastic 
bands  that  stretch  or  contract  to  accommodate  changes  in  the  environment  or  the  motion 
of  other  vehicles.  The  preliminary  ideas  on  this  line  of  research  were  presented  by  Quinlan 
and  Khatib  [30]  and  were  later  extended  by  Khatib,  Jaouni,  Chatila,  and  Laumond  [31] 
to  handle  nonholonomic  vehicles,  and  by  Brock  and  Khatib  [32]  to  address  the  multiple 
vehicle  case.  In  [32],  the  authors  explain  how  to  use  velocity  tuning  or  a  combination  of 
velocity  and  trajectory  tuning  to  resolve  conflicting  motions  after  the  individual  motions 
have  been  planned.  The  elastic  band  approach  uses  potential  fields  to  determine  the  new 
shape  of  the  trajectories  in  response  to  the  other  vehicles  or  newly  discovered  obstacles. 
We  could  again  modify  this  approach  for  the  underactuated  vehicle  by  incorporating  the 
new  dynamic  equations  to  account  for  the  underactuated  nature  of  the  motion. 
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2.1.3  Randomized  planning  techniques 

Randomized  techniques  inject  unpredictable  motions  or  search  steps  into  the  process 
to  achieve  excellent  results  in  practice.  Barraquand  and  Latombe  [33]  combine  ran¬ 
domization  with  a  potential  field  method  to  solve  the  motion  planning  problem.  Their 
approach  uses  potential  functions  to  plan  the  general  motions  of  the  robot  and  the  ran¬ 
domization  enters  the  algorithm  if  the  robot  enters  a  local  minimum  that  is  not  the  goal 
state.  The  approach  uses  a  random  walk  from  the  local  minimum  to  escape  its  region  of 
attraction.  The  robot  can  then  follow  the  potential  function  again  in  search  of  the  goal 
state.  The  authors  showed  that  the  approach  works  well  for  robots  with  many  degrees 
of  freedom,  for  complicated  environments,  and  when  multiple  robots  are  navigating  the 
same  environment.  For  the  multiple  vehicle  case,  the  authors  treated  the  vehicles  as  a 
single  higher-dimensional  system  to  perform  coordinated  planning. 

Another  randomized  technique  for  solving  motion  planning  problems  is  the  proba¬ 
bilistic  roadmap  planner  developed  by  Kavraki,  Svestka,  Latombe,  and  Overmars  [34,35]. 
This  planner  solves  the  problem  in  two  steps  for  robots  with  many  degrees  of  freedom 
operating  in  static  environments.  The  first  step  is  a  learning  phase  where  a  roadmap 
is  randomly  generated  to  infiltrate  the  free  configuration  space.  The  learning  process 
relies  on  a  fast  local  planner  to  connect  the  individual  configurations.  After  the  learning 
phase,  the  second  step  is  the  query  phase  where  the  algorithm  attempts  to  generate  a 
path  between  two  free  configurations.  The  query  phase  tries  to  connect  the  start  and  goal 
configurations  to  the  roadmap  and  then  determine  a  route  along  the  roadmap  to  con¬ 
nect  the  endpoints.  After  a  suitable  learning  period,  the  approach  can  quickly  generate 
solutions  for  complicated  robots  and  challenging  environments.  The  authors  note  that 
the  approach  should  be  customized  for  each  application  to  improve  the  results.  Since 
the  approach  contains  random  aspects,  it  is  not  possible  to  guarantee  the  performance, 
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but  Kavraki,  Kolountzakis,  and  Latombe  [36, 37]  have  made  efforts  to  quantify  the  ap¬ 
proach  and  determine  bounds  on  the  number  of  points  that  should  be  examined  to  find 
a  solution  with  a  given  probability.  This  approach  has  been  extended  to  handle  robots 
with  nonholonomic  constraints  [38]  and  when  multiple  robots  are  moving  in  the  environ¬ 
ment  [39].  See  [40]  for  a  detailed  analysis  of  the  randomized  roadmap  planner  and  the 
approach  developed  by  Barraquand  and  Latombe. 

LaValle  collaborates  with  Kuffner  [41]  to  develop  a  randomized  search  technique  that 
accounts  for  the  dynamics  of  the  vehicle  to  identify  feasible  trajectories.  The  randomized 
technique  is  called  rapidly-exploring  random  trees  (RRTs).  The  approach  builds  a  tree 
between  the  initial  and  goal  states  by  sampling  the  free  configuration  space  and  planning 
motions  between  the  samples.  The  type  of  tree  and  sampling  scheme  can  be  tailored  to 
improve  the  performance  of  the  algorithm.  The  approach  easily  incorporates  kinodynamic 
constraints  for  the  vehicle  and  obstacles  in  the  environment.  In  addition,  the  approach 
offers  attractive  advantages  in  computation  speed  and  flexibility  that  may  be  useful  when 
we  address  the  obstacle  avoidance  problem  and  planning  for  multiple  vehicles. 

The  paper  by  Hsu,  Latombe,  and  Motwani  [42]  uses  a  technique  similar  to  rapidly- 
exploring  random  trees  to  generate  paths  between  initial  and  goal  states.  The  authors 
expand  from  the  initial  and  goal  states  by  choosing  points  that  have  relatively  few  neigh¬ 
bors  and  sampling  the  free  space  around  these  points.  The  approach  then  tries  to  connect 
the  existing  tree  to  the  new  points.  The  algorithm  also  tries  to  connect  the  trees  growing 
from  the  initial  and  goal  states  together  to  complete  the  path.  Related  works  by  Kindel  et 
al.  [43]  and  Hsu  et  al.  [44]  apply  the  technique  to  systems  with  kinodynamic  constraints 
to  achieve  excellent  results.  The  authors  demonstrate  that  the  approach  performs  well 
when  the  environment  includes  multiple  moving  obstacles.  We  note  that  the  main  dif¬ 
ference  between  this  approach  and  RRTs  is  in  how  the  existing  path  is  extended  into 
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the  unexplored  regions.  This  difference  is  relatively  minor  and  both  approaches  produce 
similar  results. 

Hu  et  al.  [45]  developed  another  planning  technique  that  layers  random  motion  on 
top  of  a  deterministic  plan  to  develop  collision-free  motions.  Their  decoupled  planning 
approach  was  designed  to  resolve  potential  conflicts  between  aircraft.  The  algorithm 
efficiently  estimates  the  probability  of  a  conflict  and  then  adapts  the  motion  of  each 
aircraft  to  reduce  the  likelihood  of  a  collision.  This  randomized  approach  offers  a  potential 
solution  to  the  multiple  vehicle  planning  problem. 

The  recent  paper  by  Leven  and  Hutchinson  [46]  presents  another  variation  on  the 
probabilistic  roadmap  planning  approach.  Their  contribution  solves  the  problem  by  first 
finding  a  roadmap  to  represent  an  environment  with  no  obstacles.  The  authors  then 
determine  a  mapping  between  regions  in  the  workspace  and  segments  of  the  roadmap. 
With  these  two  preprocessing  steps  completed,  the  algorithm  can  then  plan  motions 
in  real  time  when  obstacles  are  present.  To  do  so,  the  real-time  planner  first  uses  the 
mapping  to  eliminate  the  portions  of  the  roadmap  that  are  blocked  by  obstacles.  Then  the 
planner  searches  the  remaining  roadmap  to  connect  the  initial  and  goal  configurations. 
This  approach  is  extremely  successful  at  developing  plans  for  high-dimensional  systems, 
but  it  requires  the  extensive  preprocessing  stage  to  develop  the  solutions. 

For  our  motion  planning  problem,  we  are  seeking  a  technique  that  can  quickly  generate 
paths  for  nonlinear  underactuated  vehicles,  that  can  handle  obstacles  in  the  environment, 
and  that  can  accommodate  planning  for  multiple  vehicles.  Chapter  3  presents  our  original 
solution  to  the  basic  motion  planning  problem,  but  this  approach  alone  cannot  handle 
the  obstacle  avoidance  and  multiple  vehicle  planning  problems.  The  recent  trends  in 
the  literature  indicate  that  randomized  techniques  offer  the  best  chances  for  quickly 
generating  solutions  to  these  more  demanding  problems.  Other  approaches  may  be  able 
to  guarantee  finding  solutions  when  they  exist,  but  the  computational  requirements  make 
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these  algorithms  impractical  with  current  processing  speeds.  From  the  assortment  of 
randomized  techniques  available,  we  selected  RRTs  [41]  as  a  tool  to  help  address  the 
motion  planning  problem.  The  approach  appears  to  be  sufficiently  fast  and  is  flexible 
enough  to  handle  the  variations  of  the  underactuated  vehicle  problem.  We  will  use  our 
solution  to  the  basic  motion  planning  problem  to  correct  some  of  the  deficiencies  in  the 
RRTs  algorithm  and  report  on  the  results  in  Chapter  3. 

2.2  Underactuated  Vehicle  Control 

A  fair  amount  of  research  effort  has  been  applied  to  study  the  control  problems 
associated  with  underactuated  vehicles  in  air,  land,  sea,  and  space  environments.  For  our 
contributions,  we  used  a  model  of  an  underactuated  ship  and  posed  a  tracking  problem 
inspired  by  the  work  of  Pettersen  and  Nijmeijer  [3,47].  Their  controller  is  based  on  a 
variation  of  the  backstepping  technique  and  allows  the  ship  to  recover  from  initial  errors 
to  track  a  reference  trajectory.  Their  model  does  not  include  disturbances  and  they 
have  not  proven  stability  for  the  output  feedback  controller  used  for  the  experiments 
described  in  [47].  The  theoretical  results  presented  by  Pettersen  and  Nijmeijer  rely  on 
a  set  of  transformations  that  put  their  nonlinear  system  in  chained  form  so  that  they 
can  apply  the  approach  Jiang  and  Nijmeijer  presented  in  [48].  Pettersen  and  Nijmeijer 
developed  a  different  way  to  perform  tracking  for  an  underactuated  vehicle  by  combining 
backstepping  with  averaging  as  reported  in  [49].  The  model  for  this  problem  did  not 
include  disturbances  and  the  resulting  motion  displayed  oscillations  in  the  orientation 
and  lateral  velocity  of  the  vehicle.  These  researchers  also  teamed  to  study  the  output 
feedback  tracking  problem  for  ships  in  [50],  but  this  effort  did  not  consider  underactuated 
vehicles. 
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Godhavn  developed  a  more  traditional  type  of  backstepping  controller  to  make  a 
ship  track  a  desired  trajectory  [51].  This  work  was  recently  extended  by  Toussaint, 
Ba§ar,  and  Bullo  [52]  to  account  for  vehicles  with  generalized  forces.  Godhavn’s  approach 
required  the  ship  to  move  along  straight  lines  or  arcs  of  circles  to  guarantee  stable  zero 
dynamics  for  the  nonlinear  system.  Although  our  approach  also  restricts  the  class  of 
reference  trajectories  the  vehicle  can  track,  we  allow  for  a  much  wider  range  of  motion 
than  just  straight  lines  and  arcs  of  circles.  In  addition,  under  certain  initial  conditions, 
Godhavn’s  approach  would  allow  the  ship  to  rotate  180  degrees  and  track  the  reference 
path  backwards.  Our  approach  corrects  this  deficiency,  and  the  simulations  show  that 
the  vehicle  makes  natural  maneuvers  to  recover  from  poor  initial  conditions. 

Other  results  in  underactuated  vehicle  control  come  from  Leonard  [53-55].  Leonard’s 
research  focuses  on  underwater  vehicle  control,  but  the  system  models  and  control  the¬ 
ories  are  similar  to  those  for  surface  vessels.  Her  results  rely  on  energy-based  methods 
and  exploit  the  natural  dynamics  of  the  vehicles  to  stabilize  a  steady  motion,  which  is 
also  known  as  a  relative  equilibrium. 

Bullo  has  adopted  a  geometrical  approach  for  controlling  the  motion  of  a  vehicle 
along  relative  equilibria.  His  work  with  Leonard  and  Lewis,  as  reported  in  [56-58], 
considers  underactuated  vehicles.  In  addition,  Bullo  and  Murray  [59]  have  recently  used 
the  geometric  description  to  develop  results  for  a  fully  actuated  vehicle,  and  Bullo  has 
provided  in  [60]  a  general  description  of  the  geometrical  version  of  the  problem. 

Although  the  above  references  focus  on  nautical  vessels,  underactuated  control  tech¬ 
niques  have  been  developed  for  other  vehicles  as  well.  Reyhanoglu,  van  der  Schaft, 
McClamroch,  and  Kolmanovsky  [2]  have  presented  a  general  framework  for  studying  un¬ 
deractuated  systems,  which  offers  a  compact  introduction  to  the  subject  and  includes 
applications  to  different  types  of  vehicles.  Hauser,  Sastry,  and  Meyer  [61]  used  exact 
linearization  to  control  the  motion  of  an  underactuated  air  vehicle.  Martin,  Devasia,  and 
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Paden  [62]  considered  the  same  type  of  air  vehicle  as  Hauser,  but  approached  the  problem 
using  flat  outputs  to  find  an  appropriate  controller.  Reyhanoglu  [63, 64]  and  Reyhanoglu, 
van  der  Schaft,  McClamroch,  and  Kolmanovsky  [65]  addressed  the  underactuated  space¬ 
craft  control  problem.  Underactuated  robots  have  been  discussed  in  Spong  [66, 67]  and 
the  references  therein. 


2.3  H°°- Optimal  Control 

The  final  set  of  literature  related  to  our  problem  addresses  the  solution  to  the  H°° 
-optimal  control  problem.  To  help  explain  the  rationale  for  selecting  the  combination 
of  linearization  and  H°°  design  for  our  approach,  we  will  start  by  reviewing  a  variety  of 
standard  nonlinear  control  techniques  and  describe  why  they  are  not  appropriate  for  our 
problem.  We  will  then  describe  the  literature  related  to  the  tracking  control  problem 
with  an  emphasis  on  H°°  techniques. 

2.3.1  Alternative  nonlinear  control  techniques 

The  first  nonlinear  control  approach  we  considered  for  this  problem  was  exact  lin¬ 
earization.  Isidori  [68]  provides  a  complete  description  of  the  theory  behind  this  approach 
for  single-input  single-output  systems,  as  well  as  for  multiple-input  multiple-output  sys¬ 
tems.  The  essence  of  the  approach  is  to  find  a  coordinate  transformation,  which  performs 
a  change  of  variables,  and  a  state  feedback  for  the  nonlinear  system,  such  that  in  the  new 
coordinates  and  with  the  feedback  the  resulting  system  is  linear  with  respect  to  a  new 
input.  Once  we  know  the  exact  linearization  of  the  system,  we  can  use  any  of  the  well 
established  linear  design  techniques  to  develop  a  controller  for  the  system.  We  can  then 
reverse  the  coordinate  transformation  and  the  state  feedback  rule  to  find  the  correspond¬ 
ing  controller  for  the  original  nonlinear  system.  We  cannot  use  exact  linearization  for  the 
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underactuated  vehicle  control  problem  because  the  system  does  not  satisfy  the  sufficient 
conditions  to  use  the  approach.  Specifically,  the  equations  of  motion  for  the  vehicle  do 
not  have  a  well-defined  vector  relative  degree,  which  prevents  us  from  performing  the 
exact  linearization. 

A  recent  paper  by  Devasia,  Chen,  and  Paden  [69]  is  closely  related  to  the  exact 
linearization  approach  and  deserves  a  few  comments.  The  authors  developed  a  nonlinear 
operator  that  allows  them  to  invert  a  nonlinear  system  to  reconstruct  the  input  from  the 
desired  output  trajectory.  This  problem  is  very  similar  to  the  one  we  are  addressing  here 
and  the  technique  relies  on  the  same  tools  used  for  exact  linearization.  The  significant 
drawback  with  the  approach  that  makes  it  inappropriate  for  our  problem,  however,  is 
that  the  control  law  does  not  work  very  well  when  there  are  unmodeled  dynamics  or 
disturbances. 

A  related  alternative  to  exact  linearization  is  input-output  linearization,  which  is  also 
known  as  partial  linearization  [68].  In  this  variation,  we  select  a  set  of  outputs  and  then 
find  the  coordinate  transformation  and  state  feedback  to  make  the  new  system  linear  with 
respect  to  the  outputs.  This  approach  is  especially  useful  if  we  are  interested  in  tracking 
just  a  portion  of  the  state  vector  and  do  not  need  to  control  the  remaining  states.  Input- 
output  linearization  has  the  by-product  of  generating  zero  dynamics,  which  describe  the 
internal  behavior  of  the  transformed  system.  To  develop  well-defined  results,  the  zero 
dynamics  must  be  stable.  Relating  this  approach  to  our  problem,  we  have  two  control 
inputs  available  in  the  design  and  could  perform  input-output  linearization  if  we  select 
appropriate  outputs.  The  most  appropriate  outputs  to  influence  the  motion  of  the  vehicle 
are  the  x-y  position  coordinates.  This  choice  for  outputs  does  not  lead  to  a  well-defined 
relative  degree,  so  the  approach  will  not  work.  In  addition,  using  the  Zi  position  variables 
described  in  Section  1.4  does  not  resolve  the  problem.  We  could  input-output  linearize 
the  system  if  we  chose  x  and  ip  as  the  output  variables,  but  having  the  ability  to  exactly 
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track  these  two  elements  of  the  state  vector  leads  to  trajectories  that  do  not  follow  the 
y  position  variable.  Not  being  able  to  track  the  y  position  translates  into  exceptionally 
poor  performance.  Input-output  linearization  is  feasible  for  our  system,  but  it  produces 
designs  that  do  not  necessarily  track  the  entire  configuration,  which  is  one  of  our  goals. 

Another  common  nonlinear  control  design  technique  is  integrator  backstepping.  The 
standard  reference  for  backstepping  is  the  text  by  Krstic,  Kanellakopoulos,  and  Koko- 
tovic  [70],  which  also  describes  adaptive  control  techniques.  The  basic  idea  behind  back- 
stepping  is  that  if  we  can  write  the  system  equations  with  a  certain  structure  (parametric 
strict-feedback  form),  then  there  is  an  iterative  procedure  to  build  a  controller  by  con¬ 
sidering  one  layer  of  the  system  at  a  time.  As  mentioned  in  Section  2.2,  backstepping 
has  been  used  to  address  the  nonlinear  underactuated  vehicle  problem  we  are  consider¬ 
ing  [3,51].  The  backstepping  approaches  offer  alternatives  to  our  design,  but  they  do  not 
explicitly  account  for  the  disturbances  that  enter  the  equations  of  motion.  The  track¬ 
ing  controller  we  will  present  includes  the  disturbances  in  the  design  process,  and  we 
will  demonstrate  how  it  performs  better  than  the  two  backstepping  approaches  when  we 
present  the  simulation  results  in  Chapter  6. 

Two  other  techniques  that  could  be  used  to  solve  this  type  of  nonlinear  control  prob¬ 
lem  are  gain  scheduling  and  pseudolinearization.  See  Khalil’s  text  [71]  for  a  general 
description  of  gain  scheduling,  and  the  paper  by  Reboulet  and  Champetier  [72]  for  an 
introduction  to  pseudolinearization.  Briefly,  gain  scheduling  linearizes  the  system  equa¬ 
tions  about  a  set  of  operating  points  and  then  determines  a  switching  schedule  among 
the  points  to  maintain  control  over  the  system.  The  key  challenge  in  this  approach  is  to 
design  the  switching  schedule  to  achieve  the  desired  performance.  Pseudolinearization  is 
a  related  approach,  where  a  coordinate  transformation  and  a  feedback  are  used  to  make 
the  system  such  that  when  the  new  system  is  linearized,  the  results  do  not  depend  on  the 
operating  point.  This  approach  allows  the  designers  to  develop  a  single  controller  based 
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on  a  linear  system,  and  it  does  not  require  scheduling  to  account  for  different  operating 
points.  Both  of  these  techniques  could  be  applied  to  nonlinear  underactuated  systems, 
but  we  chose  to  study  H°°  techniques  instead.  We  will  leave  the  application  of  gain 
scheduling  and  pseudolinearization  to  underactuated  systems  as  open  research  problems. 

We  have  outlined  a  variety  of  nonlinear  control  techniques  that  are  standard  ap¬ 
proaches  for  the  type  of  problem  we  are  considering.  We  have  decided  to  apply  lineariza¬ 
tion  along  with  H°°  design  techniques  to  address  the  problem  because  combining  these 
methods  offers  a  tractable  approach  with  reasonable  performance.  The  ability  of  this 
approach  to  solve  the  nonlinear  underactuated  vehicle  tracking  problem  has  not  been 
documented  and  we  are  contributing  to  fill  that  vacancy.  The  linearization  simplifies  the 
design,  but  restricts  our  analysis  so  that  we  can  only  prove  local  results.  The  H°°  tech¬ 
niques  partially  compensate  for  the  linearization  because  they  are  robust  with  respect 
to  disturbances.  Since  our  problem  statement  explicitly  includes  disturbances,  we  must 
develop  an  approach  to  attenuate  their  effect  on  the  system.  The  approach  also  allows 
us  to  account  for  the  linearization  errors  by  incorporating  the  remaining  nonlinear  terms 
into  the  disturbances.  This  approach  is  more  conservative,  but  allows  us  to  make  precise 
statements  about  the  closed-loop  performance. 

To  complete  this  section,  we  will  now  review  the  literature  directly  related  to  tracking 
control  and  the  disturbance  attenuation  problem. 

2.3.2  Tracking  control  literature 

There  is  a  great  deal  of  literature  that  addresses  the  tracking  control  and  the  output 
feedback  disturbance  attenuation  problems.  In  this  summary,  we  will  attempt  to  highlight 
some  of  the  key  contributions  to  these  fields.  We  will  mention  the  papers  that  provide 
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the  fundamental  results  and  make  more  detailed  comments  on  the  references  that  are 
directly  related  to  our  solution  of  the  problem. 

Three  papers  by  van  der  Schaft  lay  the  ground  work  for  studying  nonlinear  H°°  con¬ 
trol.  In  [73],  van  der  Schaft  shows  how  to  relate  the  disturbance  attenuation  performance 
of  a  nonlinear  system  to  the  performance  of  the  linearization  of  the  system.  The  paper 
captures  the  basic  approach  we  used  in  the  controller  design  for  the  underactuated  vehi¬ 
cles  and  explains  how  the  Riccati  equations  for  the  linearized  system  are  related  to  the 
Hamilton- Jacobi  equation  for  the  nonlinear  system.  This  first  paper  considered  time- 
invariant  systems  and  used  a  performance  index  based  on  an  infinite  time  horizon.  In 
contrast,  our  problem  has  a  time-varying  nonlinear  system  (and  linearization)  and  a  fi¬ 
nite  horizon  performance  index.  Van  der  Schaft  expands  on  these  results  in  [74],  where 
he  accounts  for  the  finite  horizon  performance  index  and  provides  a  more  detailed  de¬ 
scription  of  the  results.  In  this  paper,  Remark  24  is  relevant  to  our  problem.  In  that 
remark,  van  der  Schaft  suggests  an  approach  to  prove  that  the  solution  to  the  linearized 
problem  maintains  the  same  performance  level  as  the  solution  to  the  nonlinear  problem. 
In  addition,  the  remark  indicates  the  region  of  attraction  for  the  linearized  solution  will 
be  contained  in  the  region  of  attraction  for  the  nonlinear  solution  and  the  latter  region  is 
the  largest  one  possible.  In  the  third  paper  [75],  van  der  Schaft  summarizes  the  previous 
results  and  includes  the  solution  to  the  H°°  dynamic  output  feedback  problem.  The 
solution  to  this  problem  is  based  on  two  Riccati  equations  and  uses  the  game-theoretic 
approach  from  [76]  to  develop  the  results.  The  three  papers  by  van  der  Schaft  provide 
a  basic  introduction  to  the  nonlinear  H°°  control  problem  we  are  trying  to  solve  for  an 
underactuated  system. 

Isidori  provides  a  comprehensive  review  of  disturbance  attenuation  for  nonlinear  sys¬ 
tems  in  [77].  The  paper  develops  the  results  in  the  linear  setting  and  then  highlights  the 
similarities  in  finding  a  solution  to  the  nonlinear  problem. 
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Our  underactuated  vehicle  control  problem  involves  tracking  a  desired  trajectory, 
and  we  have  formulated  the  problem  in  terms  of  the  error  dynamics,  so  it  is  essentially 
a  regulation  problem.  Isidori  and  Byrnes  [78]  address  the  output  regulation  problem 
for  nonlinear  systems.  Their  results  reiterate  the  fact  that  the  solution  requires  solving 
a  nonlinear  partial  differential  equation.  Although  this  problem  is  generally  difficult  to 
solve,  Krener  [79]  describes  approaches  for  iteratively  constructing  the  solution  developed 
by  Isidori  and  Byrnes.  The  nonlinear  system  in  [78]  is  time-invariant  and  the  authors 
allow  the  time  horizon  to  extend  to  infinity,  so  we  cannot  apply  the  results  directly  to 
our  problem. 

To  overcome  one  of  these  limitations,  Ball,  Kachroo,  and  Krener  [80]  recently  ad¬ 
dressed  the  nonlinear  H°°  tracking  problem  and  provided  a  solution  for  the  finite  time 
horizon  case.  As  expected,  the  results  depend  on  finding  solutions  to  Hamilton- Jacobi 
inequalities.  In  addition,  the  authors  have  developed  numerical  techniques  for  estimating 
solutions  to  the  inequalities  and  promise  to  provide  more  details  about  their  methods  in 
the  future. 

Since  our  controller  designs  also  depend  on  measurement  feedback,  the  papers  by 
Isidori  and  Astolfi  [81],  and  Ball,  Helton,  and  Walker  [82]  are  of  interest.  Both  papers 
consider  the  infinite  time  horizon  versions  of  the  H°°- control  problem  for  a  nonlinear 
system  with  measurement  feedback.  In  addition,  both  papers  show  that  the  solution 
depends  on  solving  a  pair  of  partial  differential  equations,  but  the  approaches  are  slightly 
different.  In  [81],  the  authors  prove  that  the  solution  satisfies  the  required  dissipation 
inequalities  and  relate  the  results  to  the  more  familiar  linear  case.  In  [82],  the  authors 
rely  on  a  game-theoretic  approach  to  the  problem  to  describe  the  solution.  As  described 
below,  we  will  also  adopt  the  game-theoretic  approach,  since  it  provides  a  natural  setting 
to  analyze  the  time-varying  finite  horizon  problem. 
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James  and  Baras  [83]  present  a  slightly  different  solution  to  the  H°°  output  feedback 
control  problem  for  nonlinear  systems.  Their  more  general  results  show  that  the  required 
H°°  controller  is  expressed  in  terms  of  the  information  state  of  the  system.  The  informa¬ 
tion  state  is  infinite  dimensional  and  captures  observable  information  that  is  important 
for  the  controller.  The  information  state  may  not  provide  the  best  estimate  for  the  state 
of  the  system,  but  it  does  carry  the  history  which  allows  us  to  design  the  optimal  distur¬ 
bance  attenuating  controller.  The  infinite-dimensional  nature  of  the  information  state 
introduces  a  significant  obstacle  to  computing  the  solution.  James  and  Baras  [83]  devel¬ 
oped  their  results  using  discrete  time  systems  and  briefly  commented  on  how  to  adapt 
the  results  for  continuous  time  systems.  Although  this  approach  is  theoretically  sound, 
we  will  not  focus  on  it  because  it  does  not  readily  lead  to  a  practical  solution  to  our 
problem. 

The  one  paper  that  brings  together  all  of  the  keys  aspects  of  the  problem  we  are 
considering  is  by  Lu  [84] .  Lu  addresses  the  H°°  output  feedback  control  problem  for  non¬ 
linear  time- varying  systems  with  a  finite  time  horizon.  The  author  presents  the  sufficient 
conditions  to  find  a  solution  in  terms  of  the  familiar  Hamilton-Jacobi  inequalities.  As  we 
have  mentioned  before,  this  technically  accurate  solution  relying  on  partial  differential 
inequalities  is  difficult  to  implement  for  nonlinear  systems.  The  controller  design  ap¬ 
proach  presented  in  Chapter  4  uses  linearization  and  the  system  decomposition  to  avoid 
the  problems  associated  with  the  full  nonlinear  solution. 

We  have  adopted  a  game-theoretic  approach  to  address  the  control  problems,  and 
Ba§ar  and  Bernhard  [76]  provide  a  complete  account  of  the  solution  from  this  perspective 
for  both  linear  and  nonlinear  systems.  Since  this  book  is  a  single  source  that  contains  most 
of  the  results  required  for  our  disturbance  attenuation  analysis,  we  will  use  its  notation  to 
present  our  solution.  As  a  secondary  reference,  Ba§ar  and  Olsder  [85]  provide  additional 
material  on  noncooperative  dynamic  games.  We  followed  the  results  in  this  book  to 
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help  derive  the  optimal  solutions  for  affine-quadratic  disturbance  attenuation  problems. 
The  paper  by  Pan  and  Ba§ar  [86]  supplies  some  key  results  on  how  the  H°°  controllers 
developed  for  a  nominal  linear  system  are  still  optimal  when  nonlinear  perturbations  are 
introduced.  We  rely  on  these  results  to  complete  the  disturbance  attenuation  proofs  for 
our  controller  designs. 

The  H°°  controller  design  uses  linearization  about  a  trajectory  to  generate  the  solution 
and  prove  stability.  Our  results  resemble  those  originally  developed  by  Cheng  [87]  and 
Walsh,  Tilbury,  Sastry,  Murray,  and  Laumond  [88],  but  we  extend  them  to  account  for 
the  disturbances  in  the  system. 

The  final  set  of  papers  in  this  literature  review  describe  four  different  techniques  for 
obtaining  disturbance  attenuation  results  for  specific  nonlinear  systems.  Elgersma,  Stein, 
Jackson,  and  Yeichner  [89,90]  use  linearized  system  equations  to  study  H°°  controller 
designs  for  a  rigid  spacecraft.  Even  though  they  used  H°°  techniques  to  analyze  the 
system,  the  authors  reverted  to  a  simple  linear  control  law  for  the  final  design.  In  the 
second  technique,  Kang  [91]  uses  the  unique  geometric  structure  in  the  rigid  spacecraft 
problem  to  show  how  a  simple  proportional-derivative  control  law  can  be  used  to  satisfy 
the  Hamilton-Jacobi  inequalities  associated  with  the  problem.  The  geometry  of  the 
problem  enters  the  solution  by  providing  convenient  bounds  on  some  of  the  terms  that 
help  reduce  the  complexity  of  the  inequalities.  The  results  by  Dalsmo  and  Egeland  [92] 
and  Dalsmo  [93]  address  the  same  problem  and  are  extremely  similar  to  those  by  Kang, 
except  they  use  quaternions  to  express  the  system  coordinates.  The  new  coordinates 
allow  the  authors  to  extend  some  of  the  local  results  in  [91]  to  be  global  in  nature.  The 
final  disturbance  attenuation  technique  comes  from  the  paper  by  Zasadzinski,  Richard, 
Khelfi,  and  Darouach  [94],  and  applies  to  robot  manipulators.  In  this  case,  the  authors 
show  how  to  rearrange  the  equations  of  motion  for  the  system  and  incorporate  a  linear 
control  law  to  satisfy  the  Hamilton-Jacobi  inequalities.  The  solution  uses  output  feedback 
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to  perform  tracking  and  imposes  bounds  on  the  type  of  trajectories  that  can  be  followed 
to  complete  the  proofs.  These  four  techniques  provide  a  cross  section  of  the  options 
available  to  develop  control  laws,  but  they  all  rely  on  fully  actuated  controllers,  and 
there  does  not  seem  to  be  any  way  to  generalize  them  to  accommodate  underactuated 
systems.  In  view  of  this,  there  is  a  need  to  develop  new  techniques  to  address  the  problem 
of  controlling  underactuated  systems. 

This  section  described  the  standard  control  techniques  that  should  be  considered 
first  when  dealing  with  nonlinear  systems.  We  concluded  that  most  of  the  standard 
approaches  were  not  suitable  for  the  underactuated  vehicle  tracking  problem  and  we 
selected  the  combination  of  linearization  and  H°°  techniques  to  develop  a  relatively  simple 
solution  with  reasonable  performance  characteristics.  We  also  included  papers  describing 
significant  results  with  H°°  control  techniques  as  well  as  papers  addressing  the  tracking 
problem.  The  literature  review  indicates  that  our  approach  to  the  problem  is  novel  and 
shows  how  our  contributions  relate  to  the  established  results. 

2.4  Literature  Review  Summary 

We  presented  a  sample  of  the  relevant  literature  from  the  motion  planning,  underactu¬ 
ated  systems  and  #°°-optimal  control  fields  to  identify  how  the  current  work  contributes 
to  these  disciplines.  This  review  helps  establish  the  originality  of  our  results  and  also 
points  to  additional  related  research  areas  that  remain  as  open  problems.  Having  de¬ 
scribed  this  background  material,  we  are  prepared  to  present  our  solution  to  the  motion 
planning  and  control  problems  for  a  nonlinear  underactuated  vehicle. 
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CHAPTER  3 


MOTION  PLANNING  FOR  UNDERACTUATED 

VEHICLES 


The  first  step  in  the  design  process  is  to  plan  the  motion  for  the  underactuated  vehicle. 
Our  objective  is  to  generate  a  feasible  trajectory  that  joins  an  initial  configuration  with 
a  goal  configuration  in  the  alloted  time  frame.  A  feasible  trajectory  is  one  that  the 
underactuated  vehicle  could  follow  if  there  were  no  disturbances  and  the  vehicle’s  initial 
configuration  aligned  with  the  trajectory.  There  are  many  ways  to  address  this  problem, 
so  we  will  present  a  direct  approach  for  constructing  a  trajectory  using  numerical  methods 
and  H°°  techniques.  In  addition  to  solving  the  basic  motion  planning  problem,  we  will 
present  solutions  to  the  obstacle  avoidance  and  multiple  vehicle  planning  problems  and 
we  will  comment  briefly  on  the  minimum  distance  planning  problem.  Before  addressing 
these  items,  we  will  provide  a  precise  formulation  of  the  general  motion  planning  problem. 

3.1  Problem  Formulation 

To  formulate  the  basic  motion  planning  problem,  we  assume  we  are  given  the  nonlinear 
dynamics  describing  the  vehicle  we  want  to  control.  We  are  also  given  the  initial  and 
final  configurations  for  the  vehicle  as  well  as  a  time  interval  for  the  motion.  Denote  the 
initial  and  final  configurations  as  qo  and  q /,  respectively,  and  the  time  interval  as  [to, 
tf].  Our  goal  is  to  find  the  control  inputs  to  move  the  vehicle  from  q0  to  q /  during  the 
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time  interval  [to,  £/].  We  can  then  apply  these  control  inputs  to  the  equations  of  motion 
for  the  vehicle  to  generate  a  feasible  trajectory. 

For  the  basic  motion  planning  problem,  there  are  no  obstacles  in  the  environment  and 
no  other  requirements  for  the  vehicle,  except  that  its  motion  must  satisfy  the  dynamic 
equations.  We  will  introduce  obstacles  and  additional  vehicles  after  solving  the  basic 
problem.  Even  though  the  vehicle  is  underactuated,  there  are  still  an  infinite  number  of 
control  laws  that  will  move  the  vehicle  from  one  configuration  to  the  other  in  the  given 
time.  We  would  like  to  find  a  technique  for  directly  generating  the  required  controls, 
which  is  efficient  in  terms  of  the  required  computations,  the  type  of  path  followed,  and 
the  amount  of  control  effort  expended. 

We  note  that  we  have  selected  a  fixed  time  interval  for  the  motion  planning  problem. 
The  problem  can  also  be  formulated  with  the  time  interval  as  a  variable,  which  adds  an 
additional  degree  of  freedom  to  the  design.  For  example,  with  a  flexible  time  interval  the 
design  could  minimize  the  time  required  to  complete  a  motion.  We  selected  the  fixed  time 
interval  approach  because  it  is  a  realistic  requirement  for  many  applications.  Situations 
with  underactuated  vehicles  that  require  completing  a  motion  at  a  specified  time  include 
docking  a  ship  at  a  busy  port,  maneuvering  an  aircraft  arriving  at  a  gate,  and  guiding 
a  missile  to  a  target  in  a  coordinated  attack.  In  each  of  these  cases,  specifying  the  time 
interval  in  advance  is  an  important  part  of  the  design  and  it  should  not  be  left  as  a 
variable. 

Having  formulated  the  basic  motion  planning  problem,  Section  3.2  presents  a  solution 
to  the  problem  for  underactuated  nonlinear  vehicles  using  numerical  methods  and  H°° 
techniques.  Section  3.3  describes  how  to  build  upon  the  basic  solution  to  address  the 
obstacle  avoidance,  multiple  vehicle,  and  minimum  distance  planning  problems.  We 
conclude  the  chapter  with  a  brief  summary  in  Section  3.4. 


36 


3.2  Basic  Motion  Planning 


The  basic  motion  planning  algorithm  consists  of  four  main  steps  to  generate  a  feasible 
trajectory  that  moves  the  vehicle  from  qo  to  q /.  The  first  step  generates  a  polynomial 
curve  in  the  x-y  plane  that  connects  the  initial  and  final  positions  and  has  the  proper 
orientation  at  each  endpoint.  The  second  step  uses  the  curve  to  estimate  the  complete 
trajectory  by  assuming  the  vehicle’s  position  aligns  with  the  curve.  We  call  this  set  of 
configurations  the  candidate  trajectory.  Along  with  the  estimate  for  the  trajectory,  the 
second  step  also  finds  approximate  values  for  the  control  inputs  for  the  system,  which  we 
denote  as  the  candidate  inputs.  At  the  end  of  the  second  step,  we  have  a  complete  set  of 
configurations  that  take  the  vehicle  from  qo  to  q /  during  [to,  </].  The  third  step  uses  the 
candidate  inputs  to  generate  a  feasible  trajectory  for  the  vehicle.  If  this  feasible  trajectory 
approaches  the  desired  final  configuration,  then  the  fourth  step  is  not  required.  If  the 
feasible  trajectory  is  not  satisfactory,  the  fourth  step  implements  an  H°°- filter  which 
uses  the  underactuated  dynamics  of  the  vehicle  to  update  the  estimate  for  the  trajectory. 
With  an  updated  trajectory  estimate  we  can  also  improve  the  estimate  for  the  control 
inputs.  We  will  iterate  the  if°°-filter  until  the  resulting  feasible  trajectory  approaches 
the  final  boundary  condition  or  the  iteration  does  not  improve  the  solution.  We  now 
provide  a  detailed  description  of  each  step  as  it  applies  to  the  underactuated  ship  model 
presented  in  Section  1.4. 

3.2.1  Polynomial  position  curve  generation 

The  first  step  in  the  motion  planning  process  uses  one  of  two  established  techniques 
to  generate  a  polynomial  position  curve  connecting  the  initial  and  final  positions  for  the 
vehicle  with  the  correct  orientation  at  each  endpoint.  The  first  technique  uses  cubic 
splines  to  generate  the  curve  and  is  sufficient  for  most  motion  planning  problems.  The 
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second  technique  uses  Pythagorean  hodograph  curves  to  connect  the  points  and  is  slightly 
more  complicated  than  the  cubic  spline  approach.  The  additional  complexity  associated 
with  the  second  approach  may  be  justified  if  we  want  to  quickly  calculate  the  curve  length 
or  the  curvature  along  the  curve.  Appendix  A  provides  detailed  information  about  both 
techniques  for  generating  the  polynomial  position  curves. 

Both  curve  generation  techniques  require  knowledge  of  the  initial  and  final  configu¬ 
rations  for  the  vehicle,  which  are  given  as 


qo 

q  / 


u(to)  v(to)  r(t0)  x(t0) 


y(to)  ip(to) 


T 


T 


u(tf)  v(tf)  r(tf )  x{tf)  y(tf) 


We  compute  the  initial  and  final  velocities  in  the  inertial  frame,  x(t )  and  y(t),  by  using 
qo  and  q /  to  evaluate  (1.4)  and  (1.5)  at  to  and  tf.  respectively.  When  combined  with  the 
orientation  information,  the  initial  and  final  velocities  determine  the  direction  of  motion 
for  the  vehicle  at  the  endpoints.  We  use  this  information  along  with  the  x-y  coordinates 
of  the  endpoints  to  find  an  appropriate  polynomial  position  curve.  Once  we  have  the 
polynomial  expressions  for  x(t)  and  y(t),  we  can  use  them  to  estimate  the  other  states 
of  the  system. 


3.2.2  Candidate  trajectory  estimation 

The  second  step  in  the  algorithm  uses  the  polynomial  position  curve  to  estimate 
the  complete  trajectory  between  the  endpoints  and  to  estimate  a  pair  of  control  inputs 
corresponding  to  the  trajectory.  To  develop  accurate  estimates,  we  will  use  the  equations 
of  motion  for  the  system  and  make  some  reasonable  assumptions  about  the  motion  of 
the  vehicle.  We  start  with  a  few  comments  about  notation. 
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We  used  the  ‘dot’  notation  to  indicate  a  time  derivative  in  Equations  (1.1)— (1.6), 
because  these  equations  describe  how  the  variables  evolve  over  time.  We  will  use  the 
‘prime’  notation  to  denote  the  derivative  of  a  polynomial  or  an  estimated  state,  so  that 
x'(t )  is  the  derivative  of  the  polynomial  x(t).  We  will  use  the  ‘hat’  notation  to  denote  a 
state  or  configuration  estimate,  so  that  q  represents  the  estimate  of  q. 

We  note  that  we  will  use  the  Mathematica  software  to  perform  the  calculations  for 
the  trajectory  estimation  procedure.  This  software  allows  us  to  take  exact  symbolic 
derivatives  of  complicated  expressions,  which  simplifies  the  approach  and  improves  the 
accuracy  of  the  results.  We  could  use  other  software  packages,  such  as  Matlab,  to 
get  similar  results,  but  we  would  have  to  rely  on  numerical  estimates  of  the  derivatives. 
If  the  time  step  for  the  numerical  derivatives  is  small,  the  differences  between  the  two 
approaches  should  be  insignificant. 

For  the  first  step  of  the  trajectory  estimation,  we  use  the  polynomials  for  x(t)  and 
y(t)  as  the  estimates  for  the  position  of  the  vehicle,  so  that  x(t)  =  x(t)  and  y(t)  =  y(t). 
These  position  estimates  may  not  represent  a  feasible  motion  for  the  underactuated 
vehicle,  but  they  do  represent  the  type  of  curve  we  would  like  the  vehicle  to  follow  to 
satisfy  the  boundary  conditions.  We  will  treat  the  difference  between  the  x-y  curve  and 
a  similar  feasible  path  as  a  disturbance  for  the  motion  planning  algorithm  to  attenuate. 
The  iterative  if°°-filter  will  attenuate  this  disturbance  as  it  improves  the  estimate  for 
the  trajectory. 

We  will  perform  a  local  analysis  of  the  vehicle’s  motion  to  find  estimates  for  the  other 
states.  At  any  fixed  time  tx,  the  instantaneous  motion  of  the  vehicle  is  in  the  direction 
given  by  the  vector  combination  of  x'(ti)  and  y'{h)  and  is  tangent  to  the  x-y  curve.  If 
the  vehicle  were  moving  with  a  steady  motion  along  a  straight  line,  the  orientation  would 
align  with  the  direction  of  motion.  As  a  first  approximation  for  the  orientation  of  the 
vehicle,  we  assume  it  is  oriented  tangent  to  the  x-y  curve.  The  initial  estimate  for  the 
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orientation  is  given  by  xp(t)  as 


xp(t)  =  arctan  [x'(t),y'(t)] .  (3.1) 

The  function  arctan  (a,  b )  with  two  arguments  gives  the  argument  of  the  complex  number 
a  +  ib,  so  that  the  value  of  xp(t)  can  range  from  — 7r  to  7 r.  Equations  (1.6)  and  (3.1)  allow 
us  to  make  the  following  initial  estimate  for  the  angular  velocity  r(t)  as 

r(t)  =  x p'(t) 

x'{t)y"(t)-x"(t)y'(t) 
x'\t)  +  y*(t)  ■ 

Since  the  vehicle  is  underactuated,  it  cannot  maintain  an  orientation  that  is  tangent  to 
the  x-y  curve  if  the  motion  contains  any  turns.  With  this  in  mind,  we  can  use  these  initial 
estimates  for  xp  and  f  along  with  the  unactuated  dynamic  equation  (1.2)  to  develop  better 
estimates. 

The  improved  estimate  for  xp  assumes  the  vehicle  is  turning  in  a  circle  at  a  constant 
velocity  and  corrects  for  the  offset  angle  in  a  turn.  This  approach  is  an  improvement 
over  the  initial  estimate  because  it  assumes  the  vehicle  is  moving  along  an  arc  of  a 
circle  instead  of  in  a  straight  line.  The  following  analysis  is  based  on  one  presented  by 
Godhavn  [51]  for  a  similar  type  of  underactuated  vehicle. 

If  the  vehicle  is  moving  along  an  arc  of  a  circle,  we  can  use  the  following  expressions 
to  describe  its  position: 

,  x  K  .  ,  . 

x[t)  =  xo  +  —sm  (rt) 

y{t )  =  Vo  +  y[l  -  cos(ft)] 

where  [x0,  yo\  is  the  initial  position  and  K  is  the  constant  speed  of  the  vehicle.  We 
can  view  this  approximation  as  a  first-order  series  expansion  describing  the  path  of  the 
vehicle.  To  fully  describe  the  motion,  we  would  have  to  include  higher-order  terms  which 
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would  improve  the  accuracy,  but  complicate  the  analysis.  Since  we  are  performing  a 
local  analysis  for  a  short  time  interval,  the  first-order  approximation  is  sufficient.  Taking 
derivatives  of  the  expressions  for  the  positions  we  get 

x'(t )  =  K  cos(rt)  (3.3) 

y'(t)  =  Ksm(rt).  (3.4) 


We  can  now  solve  Equations  (1.4)  and  (1.5)  for  u  and  v  and  substitute  x'(t)  for  x(t)  and 
y'(t)  for  y(t)  using  (3.3)  and  (3.4)  to  get 


u(t)  ss  x'(t)  cos  V>( t )  +  y'(t)  sin  ^(t) 

=  K  |  cos  [ft]  cos  ip(t)  +  sin  [ft]  sin  ijj(t)  j 
=  K  cos  \rt  —  ^(t) 

v(t)  «  —  x'(t)  sin  ^4>(t)  +y'(t)  cos  V’(t) 

=  K  |  sin  [ft]  cos  V>(t)  -cos  [ft]  sin  ip(t)  | 
=  K  sin  | ft  — 


(3.5) 


(3.6) 


where  we  have  used  tj>(t)  as  the  improved  estimate  for  the  orientation  of  the  vehicle.  We 
will  write  ^(t)  as 


i{t)  =  -ip(t)  +  ip0(t) 


where  ipQ(t)  is  the  offset  angle  for  the  underactuated  vehicle.  This  offset  is  similar  to  a 
side-slip  angle  for  an  aircraft.  We  also  note  that  #(t)  =  ft  for  a  constant  rate  turn.  We 
now  have 


u(t) 

v(t) 


K  cos 
K  sin 


[</>(*)  -  ^(t)] 

$(t)  ~  • 


(3.7) 

(3.8) 


If  the  vehicle  is  making  a  constant  turn,  the  lateral  velocity  is  not  changing  and  we  can 
make  the  approximation  v  =  0.  From  a  local  perspective,  over  the  short  time  interval 
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[ti,  t\  +  <5],  where  S  is  small,  the  lateral  velocity  will  not  change  significantly,  which 
further  supports  the  approximation  that  the  lateral  acceleration  is  zero.  Using  this 
approximation,  substituting  (3.7)  and  (3.8)  into  the  unactuated  dynamic  equation  (1.2), 
we  get 

0  =  mvuf  —  dvv 

=  mvKr  cos(,0  —  'ip)  —  dvK  sin(^  —  tp). 

Rearranging,  we  arrive  at 

tan(i/>  = 

dv 

which  implies 

ip(t)  =  ip(t)  —  arctan  ™?p>—  .  (3.9) 

dv 

Equation  (3.9)  represents  the  improved  estimate  for  the  orientation  of  the  vehicle,  and 
accounts  for  the  typical  underactuated  behavior  as  the  vehicle  turns.  Note  that  if  the 
vehicle  is  not  turning,  then  f(t)  =  0  and  we  recover  the  initial  estimate  for  the  orientation 
given  by  ip( t ).  We  use  the  new  estimate  for  ip(t)  to  improve  the  estimate  for  r(t)  as  follows: 

r(t)  =  (3.10) 

We  now  have  expressions  for  x,  y,  and  f .  To  estimate  the  remaining  two  states, 
we  can  use  (3.5)  and  (3.6)  to  arrive  at  the  following  expressions  for  u(t)  and  v(t): 

u(t )  =  x'(t)  cos  i)(t)  +  y'(t)  sin  xj>(t)  (3-11) 

v(t)  =  sin  rj)(t)  +y'(t)  cos  ^(t)  .  (3.12) 

Now  that  we  have  estimates  for  the  complete  trajectory  q,  we  can  make  estimates 
for  the  two  control  inputs  for  the  system  by  using  Equations  (1.1)  and  (1.3)  and  the 
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derivatives  u'{t )  and  r'(t): 


U\(t )  =  u'(t)  —  muv(t)r(t)  +  duu(t)  (3.13) 

U2(t)  =  r'(t)  —  mru(t)v(t)  +  drr(t).  (3-14) 

The  state  estimation  procedure  fully  exploits  the  information  in  the  equations  of  mo¬ 
tion  to  provide  an  initial  trajectory  estimate  from  a  polynomial  x-y  position  curve.  The 
procedure  has  two  notable  features.  First,  the  approach  develops  an  accurate  estimate  for 
the  orientation  of  the  vehicle  in  a  turn  by  using  the  unactuated  dynamic  equation  (1.2). 
As  a  result,  the  approach  can  exactly  characterize  motion  along  straight  lines  and  arcs  of 
circles.  The  second  significant  feature  is  that  the  approach  provides  an  accurate  estimate 
for  the  lateral  velocity  of  the  vehicle.  The  estimation  technique  finds  nonzero  values 
for  the  lateral  velocity  because  the  vehicle  is  not  oriented  tangent  to  the  position  curve. 
These  two  features  make  the  trajectory  estimation  process  very  accurate  and  ultimately 
help  produce  feasible  trajectories  with  smaller  final  configuration  errors. 

We  also  emphasize  that  this  approach  can  be  generalized  to  other  types  of  underactu¬ 
ated  systems.  Although  we  have  focused  on  a  specific  model  to  illustrate  the  method,  we 
can  use  the  same  basic  techniques  with  other  vehicles.  The  simulations  and  discussion  in 
Chapter  6  will  demonstrate  how  easily  the  approach  can  be  adapted  to  other  vehicles. 

The  computations  presented  above  outline  the  steps  required  to  find  the  entire  can¬ 
didate  trajectory  between  q0  and  q /.  We  denote  this  candidate  trajectory  by  q (t).  We 
also  have  made  an  initial  estimate  for  the  two  control  inputs,  Ui(t)  and  w2(t),  which  we 
will  denote  as  u(t).  Finding  these  estimates  marks  the  end  of  the  second  step  in  the  mo¬ 
tion  planning  algorithm.  We  now  use  the  candidate  trajectory  and  inputs  to  construct  a 
feasible  trajectory. 
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3.2.3  Generating  the  feasible  trajectory 

The  next  step  in  the  motion  planning  process  is  to  use  the  inputs  u  =  [ui,  m2]t  to 
numerically  integrate  the  equations  of  motion  given  by  (1.1)  through  (1.6)  starting  at  qo. 
This  numerical  integration  will  generate  a  feasible  trajectory  q(i)  for  the  underactuated 
vehicle.  If  the  feasible  trajectory  approaches  the  desired  final  configuration,  then  we  have 
found  a  solution  to  the  basic  motion  planning  problem  and  can  move  on  to  the  tracking 
control  aspects  of  the  problem.  It  is  possible  that  the  feasible  trajectory  will  not  align 
with  the  estimated  trajectory  q.  Differences  between  the  two  trajectories  could  arise 
because  the  position  curve  is  significantly  different  from  a  straight  line  or  a  circular  arc. 
In  addition,  there  will  be  differences  because  we  are  using  the  fixed  initial  condition  q0 
to  generate  the  feasible  trajectory  and  it  may  not  match  the  estimated  initial  condition. 

One  way  to  reduce  the  differences  between  the  feasible  trajectory  and  the  estimated 
trajectory  is  to  use  the  estimated  initial  configuration  q(to)  to  initiate  the  numerical  in¬ 
tegration.  This  adjustment  will  introduce  a  small  error  between  the  initial  configurations 
for  the  actual  vehicle  and  the  feasible  trajectory.  If  the  configuration  error  is  within  the 
region  of  attraction  for  the  tracking  controllers,  then  this  approach  is  acceptable  and  will 
yield  a  suitable  feasible  trajectory.  To  better  understand  when  we  can  apply  this  correc¬ 
tion,  we  will  examine  the  region  of  attraction  for  the  state  feedback  tracking  controllers 
in  Section  4.6. 

There  is  another  way  to  adjust  the  initial  conditions  that  actually  eliminates  the  final 
position  and  orientation  errors,  but  can  introduce  a  potentially  large  initial  error  to  the 
system.  Suppose  we  use  the  desired  initial  conditions  and  the  estimated  control  inputs  to 
generate  a  feasible  trajectory  that  has  final  position  and  orientation  errors  xe,ye,  and  ipe. 
We  can  correct  these  errors  in  two  steps  by  rotating  and  translating  the  initial  conditions. 
We  rotate  the  initial  condition  to  eliminate  the  orientation  error  and  then  translate  the 
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rotated  system  to  remove  the  position  error.  We  can  perform  this  adjustment  because 
the  variables  x,  y,  and  ip  do  not  enter  into  the  equations  of  motion  for  the  dynamics 
involving  it,  v,  and  r.  When  we  apply  the  control  inputs  to  the  system  equations  with 
the  modified  initial  conditions  we  will  get  a  feasible  trajectory  that  exactly  satisfies  the 
intended  final  position  and  orientation  requirements  and  closely  matches  the  final  velocity 
requirements.  Unlike  the  previous  correction  technique,  this  approach  is  model  specific 
and  does  not  always  generalize  to  other  systems. 

If  either  approach  for  adjusting  the  initial  condition  introduces  a  large  configuration 
error,  then  the  tracking  controllers  may  not  be  able  to  recover  from  the  initial  error  to 
reach  the  desired  configuration.  We  must  have  another  approach  to  reduce  the  potential 
differences  between  the  feasible  trajectory  and  the  desired  final  configuration.  Subsec¬ 
tion  3.2.4  introduces  an  iterative  ff°°-filter  that  allows  us  to  use  the  vehicle’s  dynamics 
to  improve  the  estimate  for  the  trajectory,  the  estimate  for  the  control  inputs,  and  the 
resulting  feasible  trajectory. 

3.2.4  Iterative  if°°-filtering 

If  the  feasible  trajectory  generated  by  the  first  three  steps  in  the  basic  motion  plan¬ 
ning  algorithm  does  not  approach  the  desired  final  configuration,  we  can  use  an  iJ°°-filter 
to  iteratively  improve  the  results.  The  filtering  approach  relies  on  the  fact  that  we  can 
decompose  the  system  equations  into  two  subsystems  that  are  each  affine  if  the  state  of 
the  other  subsystem  is  known.  The  interlaced  subsystems  allow  us  to  compute  improved 
estimates  for  the  state  of  the  system  that  account  for  the  nonlinear  underactuated  dy¬ 
namics  of  the  vehicle.  We  can  then  use  the  new  state  estimates  to  update  the  estimates 
for  the  control  inputs  used  to  generate  a  feasible  trajectory.  The  general  approach  used 
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in  this  algorithm  is  similar  to  the  interlaced  H°°- filter  developed  for  the  imperfect  state 
measurement  tracking  controller  and  described  in  Chapter  5. 

We  will  derive  the  iterative  i?°°-filter  starting  with  the  equations  of  motion  for  the 
vehicle  and  assuming  that  the  disturbances  are  not  present.  We  have  polynomials  repre¬ 
senting  the  x-y  positions  we  want  the  vehicle  to  track,  but  since  it  is  an  underactuated 
vehicle,  it  may  not  be  able  to  do  so  exactly  because  the  motion  may  be  infeasible.  There¬ 
fore,  we  will  treat  the  x-y  curve  as  an  imperfect  measurement  of  the  position  of  the 
vehicle.  We  will  now  use  this  measurement  of  the  vehicle’s  position  and  the  equations  of 
motion  to  estimate  the  remaining  four  states  of  the  system  that  are  consistent  with  the 
measurements.  We  let  y  represent  the  measurement  vector  as  follows: 


“ 

“ 

- 

- 

X 

+ 

l 

0 

wx 

y 

0 

1 

Wy 

=  Cq  +  Ewxy 


(3.15) 


where 


q 

c 


u  v  x  y  r  ip 

0  0  1  0  0  0 

0  0  0  1  0  0 


T 


and  wx  and  wy  represent  disturbance  terms  that  capture  the  error  in  our  measurements. 
We  note  that  the  vector  q  now  has  a  special  ordering  for  the  states  of  the  system  that 
will  be  useful  when  we  decompose  the  equations. 

To  develop  the  if  “-filter,  we  will  first  decompose  the  nonlinear  system  into  two 
subsystems  with  the  property  that  each  one  is  affine  if  the  state  of  the  other  subsystem 
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is  known.  We  let 


qi  = 

U  V 

q2  = 

q  = 

qi 

q2 

Subsystem  1  is  given  by  the  four  equations 


u  =  muvr  —  duii  +  Ui  +  Wi 
i)  =  mvur  —  dvv  +  w  2 
x  =  cos(,0)  u  —  sin^)  v 
y  =  sin(,0)  u  +  cos(V’)  v 

which  can  be  written  compactly  as 

qi  =  -4n(q2)qi  +  ai(q2)  +  -Biui  +  D^wi. 

Likewise,  subsystem  2  consists  of  two  equations 

r  —  mruv  —  drr  +  U2  +  W3 
ip  =  r 


and  can  be  written  as 


q2  —  ^22(qi)q2  +  «2(qi)  +  -B2U2  +  i^2w2- 


(3.16) 


(3.17) 
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We  want  to  extract  as  much  information  about  the  nonlinear  system  as  possible,  so  we 
calculate  the  linear  portion  of  the  affine  terms  oi\  and  a2  as  follows: 


A12  — 


daiCgg) 

dq2 


0  0 
0  0 
0  0 
0  0 


A21  — 


da2(qi) 

9qi 


mrv  mru  0  0 


0  0  0  0 


We  can  now  rewrite  Equations  (3.16)  and  (3.17)  as 

q.i  =  ^n(q2)qi  +  Al2q_2  +  [<*i(q2)  -  A12q2}  +  Biui  ~h  D\ Wj 
q2  =  ^22(qi)q2  +  4nqi  +  [^(qi)  -  4uqi]  +  b2u2  +  r>2w2. 


We  can  combine  (3.18)  and  (3.19)  into  a  single  set  of  equations  as 


^11  (q2)  A12 

A2i  A22  (qi) 


q  + 


a1(q2)  -  ^i2q2 
«2(qi)  —  A2i  qi 


+ 


Bx  0 
0  B2 


where 


u  = 


L 

J 

L 

—  - 

r 

Ml 

,  W  = 

Wi 

«2 

W2 

u  + 


D 1 
0 


We  can  then  write  the  combined  subsystem  equations  compactly  as 


q  =  v4(q)q  +  o:(q)  +  Bu  +  9w. 

We  do  not  know  the  value  of  q  in  advance,  so  we  will  use  the  estimate  q,  to 
shortly,  in  the  terms  A(q)  and  a(q)  to  get 

q  =  ^4(q)q  +  a(q)  +  Bu  +  Dw  +  [A(q)q  -  A(q)q  +  a(q)  -  a(q)] . 

Using  a  change  of  variables,  we  can  rewrite  (3.21)  as 

q  =  A(q)q  +  a(q)  +  Bu  +  Dw  +  e  [a(t,  q)  4-  b(t,  q)u  +  d(t ,  q)w] 


(3.18) 

(3.19) 

0 

w 

B2 


(3.20) 
be  defined 

(3.21) 

(3.22) 
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where  e  >  0  is  small.  We  can  also  consider  the  term  in  square  brackets  in  (3.21)  [or 
equivalently  in  (3.22)]  to  be  part  of  the  disturbance  term  so  that  we  can  rewrite  (3.21) 
as 


q  =  A(q)q  +  a(q)  +  Bu  +  Dw  (3.23) 

where  w  accounts  for  w  and  the  term  in  square  brackets. 

We  will  take  (3.23)  to  be  the  affine  differential  equation  that  describes  the  motion  of 
the  vehicle.  We  can  measure  the  states  x  and  y  and  want  to  use  those  measurements  to 
estimate  the  remaining  states.  We  will  use  the  following  //“-filter  differential  equations 
to  estimate  the  states  and  compute  the  corresponding  covariance  matrix  [76,  pp.  298-300]: 

q  =  .A(q)q  +  a(q)  +  Bn  +  E(7TiV_1(y  —  (7q),  q(t0)  =  q^o)  (3.24) 

E  =  A(q)E  +  EAT(q)  -  E(CTAr_1C'  -  7_2Q)E  +  DDT,  E(i0)  =  Qo  l.  (3.25) 

In  (3.24),  we  start  the  state  estimate  at  the  given  desired  state  for  the  system.  We  could 
start  the  estimate  at  the  origin,  but  including  the  known  initial  condition  should  improve 
the  results.  Also  in  (3.24),  the  A(q)  matrix  and  the  affine  term  o:(q)  depend  on  the  state 
estimate,  which  is  available  information  and  allows  us  to  capture  the  nonlinear  dynamics 
in  an  affine  differential  equation. 

In  Equation  (3.24),  we  need  to  know  u  to  compute  the  state  estimates,  but  u  rep¬ 
resents  the  control  inputs  we  are  trying  to  determine  to  generate  a  feasible  trajectory. 
One  way  to  resolve  this  circular  problem  is  to  use  the  estimate  u  in  place  of  u  in  (3.24). 
We  can  then  implement  the  //“-filter  to  develop  a  new  estimate  for  the  configuration 
vector  for  the  vehicle.  With  the  new  configuration  estimates,  we  can  use  (3.13)  and  (3.14) 
to  update  the  estimates  for  the  control  inputs.  With  revised  estimates  for  the  control 
inputs,  we  can  repeat  the  //“-filtering  to  again  improve  the  results.  We  will  run  the  iter¬ 
ative  //“-filter  until  the  final  configuration  error  no  longer  improves  or  a  fixed  number  of 
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iterations  has  occurred.  We  limit  the  number  of  iterations  to  guarantee  that  the  iteration 
will  terminate.  These  new  estimates  for  the  control  inputs  may  be  an  improvement  over 
the  previous  estimates  that  did  not  use  the  iterative  //“-filter  and  could  generate  a  more 
accurate  feasible  trajectory.  We  will  use  the  final  results  of  the  //“-filter  to  generate  a 
feasible  trajectory  for  the  vehicle.  We  will  call  this  feasible  trajectory  the  desired  trajec¬ 
tory  for  the  tracking  controllers  and  use  a  d  subscript  to  denote  the  desired  configurations 
and  inputs. 

The  motion  planning  algorithms  use  four  steps  to  construct  control  inputs  for  the 
underactuated  vehicle.  The  inputs  generate  a  feasible  trajectory  to  make  the  vehicle 
move  from  the  initial  configuration  toward  the  final  configuration  in  the  allotted  time. 
We  will  present  examples  of  the  basic  motion  planning  algorithm  in  Section  6.1.  Before 
we  shift  our  focus  to  the  tracking  control  problem,  there  are  a  few  reasonable  extensions 
to  the  basic  motion  planning  problem  that  we  should  address. 


3.3  Extensions  to  Basic  Motion  Planning 

We  will  consider  three  extensions  to  the  basic  motion  planning  problem  that  rely  on 
some  of  the  fundamental  techniques  from  the  solution  presented  in  Section  3.2.  These 
extensions  are  to  solve  planning  problems  when  obstacles  are  present  and  when  other 
vehicles  are  in  the  environment,  as  well  as  to  handle  the  minimum  distance  planning 
problem.  As  mentioned  in  the  literature  review  in  Chapter  2,  randomized  techniques 
offer  promising  solutions  for  difficult  planning  problems  and  we  will  rely  on  one  random¬ 
ized  technique  to  find  solutions  to  two  of  the  extensions  to  the  basic  problem.  We  will 
describe  the  randomized  technique  of  rapidly-exploring  random  trees  before  we  explore 
the  solutions  to  the  modified  motion  planning  problems. 
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3.3.1  Rapidly-exploring  random  trees 


From  the  many  techniques  available  to  address  the  obstacle  avoidance  and  multiple 
vehicle  planning  problems,  we  selected  the  rapidly-exploring  random  trees  (RRTs)  ap¬ 
proach  developed  by  LaValle  [41,95]  as  the  one  to  use  for  our  application.  This  approach 
offers  the  advantages  of  easily  handling  the  underactuated  nature  of  our  problem,  auto¬ 
matically  accounting  for  obstacles,  and,  with  minor  modifications,  handling  the  multiple 
vehicle  case.  As  mentioned  in  Subsection  2.1.3,  this  randomized  technique  can  provide 
practical  answers  to  challenging  planning  problems,  but  its  random  nature  makes  it  more 
difficult  to  prove  that  the  approach  will  generate  an  appropriate  solution.  We  will  focus 
here  on  how  we  can  use  elements  of  the  basic  motion  planning  algorithm  to  improve  the 
standard  RRT  approach  and  how  the  new  technique  can  plan  motions  for  underactuated 
vehicles.  We  will  leave  the  detailed  proofs  describing  the  ability  of  the  new  approach  to 
generate  a  solution  as  an  item  for  future  research. 

There  are  many  variations  of  the  RRT  algorithm,  so  we  will  briefly  describe  the  one 
that  we  implemented  for  our  problem.  The  algorithm  builds  two  search  trees,  with  one 
starting  at  the  initial  configuration  and  the  other  starting  from  the  the  goal  configuration. 
The  trees  consist  of  nodes  that  represent  configurations  the  vehicle  could  achieve  after 
applying  an  input  for  an  incremental  time  step.  The  objective  is  to  build  the  two  trees 
towards  each  other  so  that  they  can  eventually  be  linked  to  form  a  complete  trajectory 
between  the  initial  and  goal  states.  The  key  innovation  with  the  approach  is  the  manner 
in  which  the  trees  are  extended.  At  each  time  step,  a  random  configuration  in  the  space 
of  all  possible  configurations  is  selected.  Each  state  variable  has  an  upper  and  lower  limit, 
so  there  are  bounds  on  the  values  for  the  configuration  variables.  The  algorithm  then 
searches  one  of  the  trees  to  find  the  closest  node  to  the  new  configuration.  The  distance 
between  two  configurations  is  determined  by  a  metric,  which  is  a  key  element  in  the 
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design  process.  Once  the  algorithm  identifies  the  closest  node  in  the  tree,  it  searches  the 
list  of  possible  inputs  to  find  values  that,  in  one  time  step,  will  move  the  configuration  as 
close  as  possible  to  the  new  node  without  colliding  with  an  obstacle.  The  algorithm  then 
adds  the  resulting  configuration  as  a  new  node  in  the  tree.  In  addition,  the  algorithm 
tries  to  extend  the  tree  with  the  new  node  towards  the  other  tree.  The  process  is  then 
repeated  with  the  roles  of  the  trees  swapped  and  the  whole  algorithm  is  iterated  until 
the  number  of  nodes  is  exhausted  or  the  trees  are  connected.  For  additional  details  on 
the  algorithm,  see  [41]  or  the  documentation  with  the  software. 

As  mentioned  above,  the  metric  used  to  compute  the  distance  between  two  config¬ 
urations  is  a  critical  aspect  of  the  algorithm.  The  metric  must  provide  a  reasonable 
measure  of  the  separation  between  two  configurations  and  we  must  be  able  to  compute 
it  quickly,  since  it  is  evaluated  many  times  in  the  algorithm.  The  standard  metrics  in 
the  example  models  in  the  software  can  be  calculated  very  quickly,  but  do  not  always 
provide  a  meaningful  result  for  an  underactuated  vehicle.  Likewise,  the  motion  planning 
metrics  suggested  in  the  recent  paper  by  Amato,  Bayazit,  Dale,  Jones,  and  Vallejo  [96] 
are  not  appropriate  for  an  underactuated  vehicle.  We  will  describe  how  we  changed  the 
metric  in  the  RRT  software  to  improve  the  results  of  the  algorithm  and  how  we  adapted 
the  software  to  accommodate  the  extensions  to  our  basic  motion  planning  problem. 

3.3.2  Obstacle  avoidance  motion  planning 

The  first  enhancement  to  the  basic  motion  planning  problem  is  to  introduce  obstacles 
into  the  environment.  Accounting  for  obstacles  will  make  the  situation  more  realistic 
and  add  a  significant  challenge  to  the  planning  problem.  For  the  planar  underactuated 
vehicle,  we  will  designate  obstacles  as  regions  that  the  vehicle  is  not  allowed  to  occupy. 
We  will  assume  the  obstacles  are  fixed  in  the  environment  and  the  vehicle  is  not  allowed 
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to  contact  them.  The  objective  will  be  to  find  a  feasible  trajectory  for  the  underactuated 
vehicle  to  track  that  avoids  collisions  with  the  obstacles.  Our  ability  to  control  the  vehicle 
will  determine  how  close  the  feasible  trajectory  can  come  to  an  obstacle. 

We  solved  the  problem  of  planning  motions  around  obstacles  by  modifying  the  RRT 
software  in  the  following  ways.  First  we  developed  a  model  for  the  underactuated  vehicle 
to  capture  the  dynamics  of  the  system.  The  equations  of  motion  for  this  model  match 
those  presented  in  Section  1.4.  We  set  upper  and  lower  bounds  on  the  states  for  the  system 
by  choosing  extreme  values  from  previous  motion  planning  trials.  We  then  developed  a 
set  of  inputs  for  the  system  that  are  representative  of  the  inputs  encountered  in  past 
experiments.  There  are  no  disturbances  in  the  motion  planning  simulations,  so  we  did 
not  have  to  set  values  for  them. 

The  most  significant  change  we  made  to  the  RRT  software  was  to  the  metric  used  to 
evaluate  the  separation  between  configurations.  The  standard  metrics  in  the  software  are 
based  on  a  Euclidean-type  distance  between  the  configurations.  These  metrics  are  not 
appropriate  for  an  underactuated  vehicle,  because  it  is  possible  that  two  configurations 
could  be  separated  by  a  small  Euclidean  distance  and  the  vehicle  would  have  to  travel 
a  relatively  long  trajectory  to  move  between  the  two  configurations.  The  new  metric 
tries  to  compensate  for  this  deficiency  by  including  a  measure  of  the  path  length  for  the 
trajectory  the  vehicle  would  actually  follow.  Given  two  configurations,  the  new  metric 
computes  the  length  of  a  Pythagorean  hodograph  curve  connecting  the  two  positions, 
with  the  curve  having  the  proper  tangent  at  each  endpoint.  The  tangents  are  determined 
by  the  vehicle’s  velocity  and  orientation  at  the  endpoints.  The  nature  of  the  Pythagorean 
hodograph  curve  is  such  that  we  do  not  have  to  compute  the  actual  polynomials  for  x(t) 
and  y(t )  to  determine  the  path  length,  which  helps  reduce  the  processing  time.  The  new 
metric  sums  the  square  of  the  path  length  with  the  squares  of  the  errors  in  the  other 
four  state  variables  and  then  takes  the  square  root  of  the  result.  This  metric  attempts 
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to  provide  a  more  reasonable  estimate  of  the  separation  between  two  configurations  than 
the  simple  Euclidean  metrics,  and  our  simulations  indicate  it  can  improve  the  results  of 
the  RRT  algorithm. 

We  solved  an  obstacle  avoidance  problem  for  the  underactuated  vehicle  using  the 
modified  RRT  software  with  both  the  standard  metric  and  the  new  Pythagorean  hodo- 
graph  curve  metric.  Since  the  approach  is  random,  we  computed  the  solution  20  times  for 
each  approach  to  get  a  sample  of  the  results.  In  each  case,  we  allowed  the  algorithm  to 
examine  up  to  10  000  nodes  in  each  tree  before  stopping  the  simulation.  If  the  simulation 
did  not  find  a  solution  within  10  000  steps,  we  counted  the  trial  as  a  failure.  We  note 
that,  given  enough  steps,  both  approaches  have  a  high  probability  of  finding  a  solution, 
so  allowing  the  simulations  to  continue  would  likely  have  generated  feasible  trajectories. 
We  terminated  the  simulations  at  10  000  steps  to  help  limit  the  time  required  to  collect 
the  data.  Table  3.1  summarizes  some  of  the  key  statistics  from  the  20  trials  and  example 
solutions  are  shown  in  Chapter  6.  Note  that  the  parameter  Gdist  that  determines  the 
maximum  separation  between  the  two  trees  before  they  can  be  connected  is  different  for 
the  two  metrics.  We  originally  used  the  same  value  for  both  metrics,  but  the  algorithm 

Table  3.1  Statistics  from  the  solutions  to  the  obstacle  avoidance  motion  planning  prob¬ 
lem  using  the  RRT  software.  We  used  each  metric  to  attempt  to  generate  20  solutions 
to  the  problem.  We  classified  the  trial  as  a  failure  if  there  was  no  solution  after  10  000 
steps. 

Standard  Metric  PH  Curve  Metric 


Incremental  time  step  (AT) 

0.2 

0.2 

Maximum  separation  to  connect  (Gdist) 

0.2 

0.5 

Failures 

7 

5 

Average  nodes  for  20  trials 

5686.15 

4284.3 

Average  nodes  in  successful  trials 

4745.4 

3065.3 

Average  path  length 

15.83 

11.15 

Average  maximum  jump 

0.383 

0.303 
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with  the  standard  metric  and  Gdist  =  0.5  would  produce  unreasonable  solutions  in  which 
the  vehicle  would  make  a  large  position  jump  between  the  trees.  In  some  cases,  the  jump 
would  even  pass  through  obstacles,  so  we  lowered  the  value  to  improve  the  performance. 
The  resulting  algorithms  had  roughly  the  same  number  of  failures,  so  the  settings  appear 
to  be  comparable. 

There  are  three  advantages  to  using  the  Pythagorean  hodograph  curve  metric  com¬ 
pared  to  the  standard  metric.  First,  the  algorithm  could  find  solutions  with  fewer  nodes 
in  the  trees,  which  translates  into  fewer  exploration  steps.  Second,  the  resulting  trajec¬ 
tories  are  usually  shorter  than  with  the  standard  metric,  so  the  motion  is  more  efficient. 
Finally,  the  maximum  jump  between  any  two  nodes  is  usually  smaller  with  the  new  met¬ 
ric,  so  the  paths  are  smoother  and  it  will  be  easier  for  the  tracking  controller  to  follow 
the  trajectories.  Note  that  the  maximum  jump  is  usually  smaller  for  the  Pythagorean 
hodograph  curve  method,  even  though  we  allowed  a  larger  value  for  Gdist  than  with  the 
standard  metric. 

The  20  trials  provide  a  fair  sample  of  the  two  approaches  and  indicate  that  the 
metric  based  on  a  Pythagorean  hodograph  curve  offers  some  improvements  over  the 
standard  approaches.  The  new  metric  requires  more  computational  time  because  it  is 
more  involved  than  the  Euclidean  metric,  but  the  improved  planning  solutions  may  be 
worth  the  additional  time.  In  addition,  we  have  not  optimized  the  code  to  compute  the 
Pythagorean  hodograph  curve  length,  and  enhanced  programming  may  be  able  to  narrow 
the  gap  between  the  two  approaches. 

As  a  final  remark  for  this  version  of  the  motion  planning  problem,  we  note  that 
we  can  smooth  the  trajectories  generated  by  the  RRT  software  to  improve  the  motion 
of  the  vehicle.  The  trajectories  generated  by  the  RRT  algorithm  satisfy  the  motion 
planning  requirements,  but  the  resulting  motion  may  be  very  rough  because  the  inputs 
are  randomly  selected  at  each  time  step.  There  are  two  simple  methods  for  smoothing 
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that  are  based  on  the  basic  motion  planning  algorithm.  The  first  smoothing  method 
applies  the  iterative  i7°°-filter  to  the  feasible  trajectory  and  we  will  refer  to  it  as  the 
filtering  approach.  The  resulting  trajectory  is  smoother  than  the  original  and  retains 
the  overall  shape,  so  it  will  likely  avoid  the  obstacles  and  other  vehicles,  as  originally 
planned.  In  addition,  the  filtered  trajectory  does  not  have  a  discontinuity  at  the  point 
where  the  two  trees  in  the  RRT  algorithm  are  connected.  Finally,  the  new  trajectory  is 
still  feasible,  so  we  can  use  the  tracking  control  algorithms  discussed  in  Chapters  4  and  5 
to  follow  it. 

The  second  approach  for  smoothing  the  original  trajectory  also  uses  elements  of  the 
basic  motion  planning  algorithm.  In  this  case,  we  sample  the  original  trajectory  and  then 
use  the  basic  planning  algorithm  to  design  motions  between  the  sampled  configurations. 
We  will  refer  to  this  technique  as  the  sampling  approach.  With  this  approach,  the  new 
segments  are  feasible  paths  and  we  can  apply  the  tracking  control  algorithms  to  make 
the  underactuated  vehicle  follow  the  new  trajectories.  It  is  possible  that  the  smoothed 
trajectory  could  collide  with  the  obstacles  or  other  vehicles,  but  this  problem  could  be 
corrected  by  sampling  more  points  in  the  trajectory  to  maintain  its  shape  in  critical  areas. 
At  each  transition  point,  we  initialize  the  tracking  controller  with  the  final  configuration 
from  the  previous  segment  to  maintain  continuity.  The  resulting  trajectories  are  again 
very  smooth  and  maintain  the  overall  shapes  developed  in  the  original  designs. 

The  sampling  approach  to  smoothing  offers  a  couple  of  advantages  over  the  filtering 
technique.  First,  with  the  sampling  approach  the  resulting  control  inputs  will  be  smooth 
functions  of  time  because  they  are  generated  by  the  tracking  control  algorithm.  With 
the  filtering  technique,  the  inputs  were  based  on  the  randomly  selected  inputs  generated 
by  the  RRT  algorithm  and  may  be  difficult  to  implement  if  there  is  fast  switching  in  the 
input  signal.  The  other  key  advantage  with  the  sampling  approach  is  that  we  can  use 
the  built-in  features  of  the  basic  motion  planning  algorithm  to  adjust  the  time  interval 
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for  the  motion.  In  our  implementation  of  the  RRT  software,  we  did  not  include  any 
constraints  to  enforce  a  time  interval  on  the  motion.  Using  the  sampling  approach  to 
smooth  the  motion  gives  us  the  opportunity  to  correct  this  problem,  which  we  cannot 
easily  accomplish  with  the  filtering  approach. 

3.3.3  Multiple  vehicle  motion  planning 

We  now  consider  the  problem  of  planning  motions  for  multiple  vehicles  in  the  presence 
of  obstacles.  The  vehicles  may  have  conflicting  requirements  that  cause  their  planned 
trajectories  to  intersect.  Our  goal  is  to  identify  feasible  trajectories  for  each  vehicle 
that  satisfy  the  motion  requirements  and  do  not  cause  collisions.  There  are  two  basic 
strategies  for  addressing  the  multiple  vehicle  problem  [6].  The  first  approach  is  called 
centralized  planning  and  works  by  combining  the  individual  vehicles  into  a  single  higher¬ 
dimensional  system  and  then  performing  planning  for  the  larger  system.  This  approach 
is  relatively  simple  to  implement  and  can  use  standard  motion  planning  techniques  on 
the  composite  system,  but  increasing  the  dimensionality  of  the  system  can  drastically 
increase  the  computational  requirements.  The  second  main  approach  is  to  independently 
plan  trajectories  for  the  individual  vehicles  and  then  identify  and  resolve  any  possible 
conflicts.  This  type  of  planning  is  known  as  decoupled  planning.  The  method  operates 
with  lower  dimensional  systems,  so  it  may  be  able  to  generate  solutions  faster  than  the 
centralized  approach,  but  there  are  situations  where  this  approach  may  not  be  able  to 
resolve  the  conflicts  and  fails  to  finds  a  solution. 

We  considered  both  approaches  for  the  multiple  underactuated  vehicle  planning  prob¬ 
lem  and  selected  the  centralized  approach  to  demonstrate  our  results.  The  centralized 
approach  is  easier  to  implement  than  a  decoupled,  decentralized  one,  and  requires  fewer 
modifications  to  the  control  algorithms  presented  in  Chapters  4  and  5.  More  specifically, 
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decoupled  planning  techniques  often  rely  on  time  scaling  to  resolve  conflicting  motions. 
Making  a  time  scale  adjustment  would  disrupt  the  feedback  control  laws  that  are  based 
on  a  solution  to  a  generalized  Riccati  differential  equation  with  a  fixed  time  scale. 

We  changed  the  RRT  software  to  develop  a  centralized  planner  for  multiple  vehicles 
with  obstacles  in  the  environment.  We  implemented  the  approach  for  the  two- vehicle  case 
only,  simply  because  it  depicts  all  features  of  the  multiple  vehicle  scenario,  and  including 
additional  vehicles  does  not  bring  in  anything  conceptually  new.  We  emphasize,  however, 
that  the  time  required  to  solve  the  motion  planning  problem  grows  exponentially  with 
the  dimension  of  the  composite  configuration  space  for  the  system  [6,  p.  377],  so  adding 
more  vehicles  will  significantly  increase  the  processing  time. 

To  modify  the  RRT  software,  we  doubled  the  number  of  state  variables  and  inputs  to 
account  for  the  second  vehicle.  We  then  replicated  the  dynamic  equations  for  the  second 
vehicle  and  established  another  set  of  initial  and  goal  configurations.  We  modified  the 
metric  computation  to  include  the  path  length  for  each  vehicle  and  the  sum  of  the  squared 
errors  for  the  velocity  and  orientation  states.  We  still  used  Pythagorean  hodograph  curves 
to  compute  the  path  lengths.  The  two- vehicle  problem  required  one  significant  change  to 
the  tree-building  algorithm  to  help  limit  the  processing  time.  The  algorithm  is  the  same 
up  through  the  point  where  the  nearest  node  in  a  tree  is  selected.  Once  the  nearest  node 
is  identified,  the  algorithm  selects  an  input  at  random  and  extends  the  tree  using  this 
input.  This  random  selection  eliminates  the  need  to  check  every  input  to  find  the  best 
one,  which  is  a  significant  savings  in  processing  time.  Since  we  have  doubled  the  number 
of  inputs,  we  have  squared  the  number  of  input  combinations,  and  checking  each  one 
is  a  substantial  processing  burden.  The  random  choice  for  the  input  does  not  penalize 
the  ability  of  the  tree  to  search  for  a  solution,  since  finding  the  closest  node  is  more 
important  than  the  motion  after  the  node  is  found.  The  final  change  to  the  software 
was  to  include  collision  checking  between  the  two  vehicles.  Since  the  state  of  the  system 
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includes  the  position  of  each  vehicle,  we  can  easily  eliminate  configurations  where  the 
distance  between  the  two  vehicles  drops  below  the  minimum  required  separation. 

We  executed  the  modified  RRT  motion  planning  algorithm  and  generated  viable  so¬ 
lutions  for  the  two-vehicle  problem  when  obstacles  were  present.  The  vehicles  avoided 
each  other  and  the  obstacles  as  they  successfully  moved  from  the  initial  states  to  the 
goal  states.  The  simulation  results  are  presented  in  Chapter  6  and  include  examples  of 
smoothed  trajectories  using  the  techniques  described  in  Subsection  3.3.2.  For  a  compar¬ 
ison,  we  also  attempted  to  solve  this  problem  using  the  standard  Euclidean-type  metric. 
After  several  extensive  experiments  with  trees  that  contained  over  30  000  nodes,  we  were 
not  able  to  find  a  solution  to  the  two  vehicle  problem  using  the  Euclidean  metric.  These 
results  again  indicate  that  the  Pythagorean  hodograph  curve  length  is  a  valuable  addition 
to  the  metric  used  in  the  RRT  algorithm. 

3.3.4  Minimum  distance  path  planning 

The  final  extension  to  the  motion  planning  algorithms  is  to  find  the  shortest  trajec¬ 
tories  that  connect  the  initial  and  final  configurations.  Using  the  model  presented  in 
Section  1.4,  we  can  construct  a  trivial  solution  to  the  minimum  distance  planning  prob¬ 
lem  by  manipulating  the  two  inputs  to  bring  the  vehicle  to  rest,  orient  it  towards  the  goal 
position,  move  the  vehicle  along  a  straight  line  to  the  goal  position,  bring  the  vehicle  to 
rest  again,  and  then  adjust  its  orientation.  Since  this  trivial  solution  is  not  very  practi¬ 
cal,  we  will  formulate  a  more  meaningful  minimum  distance  problem  by  introducing  two 
assumptions.  We  assume  the  vehicle  is  always  moving  forward  with  a  constant  velocity, 
and  that  is  has  an  upper  bound  on  how  quickly  it  can  turn.  The  literature  review  in 
Section  2.1  has  discussed  additional  formulations  of  this  problem.  For  a  planar  vehicle, 
this  problem  has  a  known  solution  where  the  path  consists  of  straight  line  segments  and 
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arcs  of  circles  [97].  We  will  describe  how  to  incorporate  this  solution  with  our  basic 
motion  planning  algorithm. 

We  can  solve  the  minimum  distance  planning  problem  by  making  a  slight  modification 
to  the  basic  planning  algorithm.  The  modification  is  to  develop  a  candidate  position 
path  composed  of  straight  lines  and  arcs  of  circles  and  then  approximate  the  path  with 
a  polynomial  to  develop  explicit  expressions  for  x(t )  and  y(t).  The  path  segments  would 
use  seventh-order  polynomials  to  maintain  smooth  derivatives  at  the  transition  points 
between  line  segments  and  circular  arcs.  The  standard  mathematical  software  packages, 
such  as  Mathematica  and  Matlab,  can  readily  compute  these  polynomials  with  built- 
in  functions.  This  approach  was  originally  proposed  by  Godhavn  [51]  and  is  a  simple 
extension  to  the  basic  algorithm,  which  already  requires  polynomial  expressions  for  x(t ) 
and  y(t).  We  can  then  proceed  with  the  remainder  of  the  basic  motion  planning  process 
as  described  in  Section  3.2.  This  solution  to  the  minimal  distance  planning  problem 
provides  a  final  example  of  how  the  basic  solution  to  the  motion  planning  problem  is 
extremely  versatile  and  can  be  modified  to  address  several  common  planning  situations. 

3.4  Motion  Planning  Summary 

We  have  presented  in  this  chapter  a  set  of  motion  planning  algorithms  that  generate 
feasible  trajectories  for  an  underactuated  vehicle.  The  solutions  address  the  basic  motion 
planning  algorithm,  the  situations  where  obstacles  or  other  vehicles  are  present  in  the 
environment,  and  the  minimum  distance  planning  problem.  The  solution  to  the  basic 
problem  uses  a  polynomial  position  curve  to  estimate  the  trajectory  the  vehicle  follows 
and  incorporates  an  iterative  tf°°-filter  to  improve  the  trajectory  estimate.  The  obstacle 
avoidance  and  multiple  vehicle  problems  rely  on  rapidly-exploring  random  trees  to  gen¬ 
erate  an  initial  solution,  which  can  later  be  smoothed  to  find  a  more  realistic  trajectory. 
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The  solution  to  the  minimum  distance  planning  problem  uses  an  established  technique  to 
modify  the  basic  algorithm.  Now  that  we  have  techniques  to  create  a  feasible  trajectory, 
we  want  to  make  the  underactuated  vehicle  follow  it.  The  next  chapter  will  explain  how 
we  can  make  the  vehicle  track  a  trajectory  even  when  there  are  disturbances  affecting  the 
system  and  the  initial  configuration  does  not  match  the  expected  initial  configuration. 
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CHAPTER  4 


PERFECT  STATE  FEEDBACK  TRACKING 

CONTROL 


The  perfect  state  feedback  tracking  control  law  for  the  underactuated  vehicle  is  the 
second  step  in  the  overall  planning  and  controlling  design  process.  The  controller  is  based 
on  //°°-optimal  design  techniques  applied  to  a  linearized  version  of  the  system  model  and 
forces  the  vehicle  to  follow  the  feasible  desired  trajectory  while  attenuating  the  effect  of 
disturbances  on  the  system.  We  prove  in  this  chapter  that  the  solution  to  the  linearized 
version  of  the  problem  provides  a  locally  exponentially  stabilizing  control  law  for  the 
nonlinear  system,  and  establish  conditions  under  which  the  solution  is  valid.  In  addition, 
we  examine  how  the  disturbance  attenuation  parameter  7  affects  the  performance  of  the 
controller.  We  will  also  discuss  how  the  controller  design  technique  applies  equally  to 
minimum  phase  and  nonminimum  phase  systems.  Finally,  we  will  use  two  methods  to 
estimate  the  region  of  attraction  for  the  state  feedback  control  law. 

4.1  Problem  Formulation 

Our  current  objective  is  to  design  a  control  law  to  track  a  feasible  trajectory  and  to 
reject  the  effect  of  disturbances.  We  will  assume  that  the  feasible  trajectory  has  already 
been  created  and  that  we  have  full  knowledge  of  the  required  control  inputs.  At  this 
stage,  we  also  assume  we  can  use  the  full  state  vector  in  the  feedback  control  law,  which 
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corresponds  to  the  perfect  state  measurement  case.  Chapter  5  will  discuss  how  to  develop 
a  controller  for  the  imperfect  state  measurement  case. 

Using  the  compact  notation,  we  recall  the  relevant  equations  for  the  vehicle  from 
Chapter  1.  The  equations  of  motion  for  the  vehicle  are  given  by 

q(*)  =  fit,  q)  +  B(t)u(t)  +  D(t)w(t),  q(fo)  =  qo  (4.1) 

where  q  €  Mn  is  the  configuration  of  the  vehicle,  u  €  Mm  is  the  control  input  vector, 
w  €  IBP  is  the  disturbance  vector,  and  f(t,  q)  is  a  vector-valued  nonlinear  function. 
The  matrices  B(t)  and  D(t)  are  (possibly  time- varying)  matrices  with  the  appropriate 
dimensions.  The  initial  condition  qo  does  not  necessarily  agree  with  the  desired  initial 
configuration  for  the  vehicle.  (To  keep  the  results  as  general  as  possible,  we  do  not 
specialize  the  description  of  the  vectors  and  matrices  to  match  the  model  presented  in 
Chapter  1.  This  more  general  description  will  make  it  easier  to  apply  the  results  to  other 
systems  in  the  future.) 

The  desired  system  uses  the  control  inputs  u d(t)  from  the  motion  planning  algorithm 
to  generate  the  feasible  trajectory  we  want  to  track.  The  desired  trajectory  is  represented 
by  the  differential  equation 

q d(t)  =  f(t,  q d)  +  B(t)ud(t),  q d(to)  =  q</o  (4.2) 

where  we  assume  that  the  initial  condition  q<*o  is  known.  We  want  the  actual  trajectory 
to  approach  the  desired  trajectory,  so  it  makes  sense  to  consider  the  error  dynamics.  If 
we  subtract  (4.2)  from  (4.1)  we  arrive  at  the  equations  for  the  error  system 

q e(t)  =  fe{t,  qe)  +  B(t)ue(t)  +  D(t)w(t),  qe(t0)  =  qe0  (4.3) 

where  qe  =  q  -  qd  and  ue  =  u  -  ud.  Note  that  in  (4.3)  the  matrices  B(t)  and  D(t) 
are  the  same  as  in  (4.1)  because  the  control  inputs  and  disturbances  enter  the  equations 
linearly. 
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Tracking  the  desired  trajectory  is  equivalent  to  finding  a  set  of  control  inputs  u e(t)  for 
the  error  system  to  drive  the  error  state  to  the  origin  and  keep  it  there.  If  we  can  find  these 
control  inputs,  then  we  can  calculate  the  tracking  control  inputs  for  the  underactuated 
vehicle  as  u  =  +  ue.  Our  control  design  approach  uses  linearization  about  the  desired 

trajectory  to  find  an  H°° -optimal  controller. 


4.2  Linearized  H°°  Controller  Design 

Our  objective  is  to  develop  a  control  law  that  drives  the  error  state  qe  to  the  origin 
even  when  disturbances  are  encountered.  The  tracking  controller  uses  a  relatively  sim¬ 
ple  linearization  approach  to  achieve  surprisingly  good  results.  To  our  knowledge,  this 
technique  for  underactuated  vehicle  tracking  has  not  been  reported  in  the  literature.  We 
start  with  error  differential  equations  in  (4.3)  and  linearize  the  nonlinear  system  about 
the  origin  qe  =  0  to  get  the  new  system  matrices 

M*)=8fit'—  .  Be(t)=B(t),  De(t)=D(t).  (4.4) 

dqe  qe=0 

Note  that  Be(t )  =  B(t)  and  De(t)  =  D(t)  because  we  assumed  in  the  problem  formulation 
that  the  control  and  disturbance  enter  the  system  dynamics  linearly.  Also  note  that 
linearizing  the  error  equations  about  the  origin  is  equivalent  to  linearizing  the  actual 
system  equations  about  the  desired  trajectory.  We  can  now  write  (4.3)  as 

qe  =  Ae(t)qe  +  Be(t)ue  +  De(t)w  +  [fe(t,  qe)  —  Ae(t)qe] ,  qe(*o)  =  qeo  (4.5) 


where  the  term  in  square  brackets  is  the  remaining  nonlinear  portion  of  the  system  and 
is  o(|qe|),  so  that  we  have 


lim 

qe|-A0 


o(|qe|) 

|qe| 


=  o. 
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We  will  start  the  controller  design  process  by  formulating  an  optimal  disturbance 
attenuation  problem  and  then  modify  the  results  to  prove  local  exponential  stability.  For 
the  initial  design,  we  will  ignore  the  nonlinear  term  in  square  brackets  in  (4.5),  but  we 
will  account  for  it  in  our  stability  analysis. 

If  we  associate  a  quadratic  performance  index  with  the  linear  portion  of  the  system 
given  in  (4.5),  we  can  compute  a  solution  to  the  H°°  control  problem  and  find  the 
optimal  disturbance  attenuating  controller  for  the  linear  problem.  Accordingly,  we  select 
the  performance  index  to  be 

L(ue,w)  =  |q e(tf)\%f  +  [  {M*)|g(t)  +  M*)|2}  dt  (4.6) 

J  to 

where  Qf  >  0,  Q(t)  >  0,  and  t  e  [to,tf].  Then,  the  related  parametrized  soft-constrained 
performance  index  is 

L7(ue,  w)  =  \qe(tf) \2Qf  +  f  {|qeC0l<2(t)  +  |ue(*)|2  -  72|w(t)|2}  dt  (4.7) 

J  to 

where  7  >  0.  The  solution  to  the  H°°  control  problem  with  full  state  measurements 
depends  on  finding  a  unique  positive  definite  solution  Zj(t)  to  the  generalized  Riccati 
differential  equation  (GRDE) 

Z  +  AjZ  +  ZAe  +  Q-Z  (BeBj  -  7-2£>eDj)  Z  =  0,  Z(tf)  =  Qf.  (4.8) 

Following  the  development  in  [76],  we  define  the  infimum  of  values  for  7  that  allow  a 
solution  to  (4.8)  as 

7*  :=  inf{7  >  0  :  The  GRDE  (4.8)  does  not  have  a  conjugate  point  on  [0,i/]}.  (4.9) 

As  we  mentioned  earlier,  we  have  assumed  in  this  development  that  the  entire  state  is 
available  for  the  feedback  controller  so  that  there  is  a  closed-loop  perfect-state  information 
structure  for  the  corresponding  linear-quadratic  zero-sum  differential  game  on  the  time 
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interval  [t0,  tf].  As  described  in  [76,  Appendix  A],  there  always  exists  a  7*  >  0  such 
that  for  any  7  >  7*  we  can  find  a  positive  definite  solution  to  (4.8).  The  solution  to  the 
GRDE  leads  to  a  unique  feedback  saddle-point  solution  to  the  differential  game,  with 

u*(t,qe(t))  =  -BT(t)Z7(t)qe(t)  (4.10) 

w*(t,qe(t))  =  7-2DT(t)Zr(t)qe(t),  t>t0  (4.11) 

where  u*  and  w*  are  the  optimal  control  and  worst-case  disturbance,  respectively. 

We  can  solve  (4.8)  in  reverse  time  for  Z^(t)  because  Ae  depends  on  only  the  desired 
trajectory,  which  we  assumed  can  be  computed  in  advance,  and  we  can  pick  7  >  7* 
and  Qf  >  0  in  our  design.  The  control  input  u*  provides  an  optimal  controller  for  the 
linearized  error  equations  (4.5),  so  we  will  set  ue  =  u*.  We  construct  the  locally  optimal 
solution  for  (4.1)  by  summing  ue(t)  +  u d(t)  =:  u(t),  since  we  know  u d(t)  in  advance.  We 
can  then  apply 

u (t)  =  u d(t)  -  RT(t)Z7(t) qe(t)  (4.12) 

as  the  state  feedback  controller  for  the  full  nonlinear  system. 

Note  that  we  are  using  the  linearized  controller  for  the  nonlinear  system.  This  con¬ 
troller  could  cause  the  region  of  attraction  to  shrink  significantly  compared  to  the  non¬ 
linear  state  feedback  controller  that  solves  the  Hamilton-Jacobi-Isaacs  (HJI)  inequality 
associated  with  the  problem  [76].  Using  the  above  formulation,  the  corresponding  HJI  is 
a  partial  differential  inequality  given  by 

e)  >  m-n  max  [Je  -1-  Be ue  +  De w]  +  | qe | q  +  ]ue|2  -  72|w|2} 

dt  u eu  wew  [  dqe  J 

V(tf,qe )  =  q(qe)  (4-13) 
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where  V(t,  qe)  is  a  continuously  differentiable  value  function.  If  we  could  solve  this 
inequality,  the  optimal  state  feedback  control  law  would  be 


u*(q»)  =  -\b1 


dV(t^)V 

3q.  ) 


(4.14) 


In  general,  for  the  time- varying  problems  we  will  consider,  Equation  (4.13)  cannot  be 
solved  analytically,  but  a  numerical  solution  is  possible.  We  opted  to  use  a  linearization 
approach  to  find  a  reasonable  control  law  instead  of  the  approximate  numerical  solution. 

The  above  controller  design  is  a  relatively  straightforward  approach  to  the  problem 
and  the  simulations  in  Chapter  6  will  demonstrate  that  it  leads  to  a  closed-loop  system 
with  reasonable  tracking  performance.  One  deficiency  with  this  approach  is  that  it  is 
difficult  to  prove  global  closed-loop  stability  for  the  nonlinear  system.  We  will  focus  on 
the  local  analysis,  so  there  is  a  direct  proof  for  stability. 

Before  we  present  the  stability  proof,  we  make  one  simple  adjustment  to  the  control 
law,  which  involves  introducing  an  additional  design  parameter  /c(t),  which  is  taken  to 
be  positive  for  all  t  G  [to,tf]: 


u.  (fj  =  -K(t)/jr 


(4.15) 


This  control  law  is  just  a  scaled  version  of  the  H°°  control  law  and  offers  some  additional 
flexibility  in  the  design.  The  modified  control  law  also  offers  reasonable  tracking  perfor¬ 
mance.  In  Section  4.3  we  will  prove  that  (4.15)  leads  to  a  locally  exponentially  stable 
closed- loop  system  under  mild  restrictions  on  K,(t). 


4.3  Local  Exponential  Stability 

We  have  already  outlined  the  controller  design  process  and  now  we  want  to  formally 
analyze  how  well  the  proposed  controller  performs.  To  prove  that  the  controller  provides 
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reasonable  tracking  performance,  we  examine  the  stability  of  the  closed-loop  system 
without  any  disturbances. 

We  will  work  directly  with  the  error  equations  for  the  system  and  we  rewrite  (4.5) 
without  the  disturbances  to  get 

qe  =  Aeqe  + Beue  +  o(\qe\).  (4.16) 

Assume  we  have  picked  Q  =  q  I,  Qf  =  qj  I,  where  q  and  qj  are  positive  scalar  constants, 
and  7  >  7*  to  solve  (4.8)  for  Z^(t).  If  we  apply  control  law  (4.15)  and  suppress  the  7 
subscript  on  Z7,  we  get  the  closed-loop  system 

qe  =  (Ae  -  nBeBjZ )  qe  +  o(|qe|).  (4.17) 

We  choose  a  candidate  Lyapunov  function 

V(t,qe)  =  cgZ(t)qe  (4.18) 

and  note  that  for  all  t  €  [to,  tf]  we  have  that  V (t,  qe)  is  positive  definite  and  radially 
unbounded  because  Z(t)  >  0  for  t  e  [to,t/].  After  solving  for  Z(t),  we  can  also  find 
real- valued  functions  m(t )  and  M (t)  and  constants  m  and  M  such  that 

0  <  ml  <  m(t)  I  <  Z(t)  <  M(t)  I  <  M I.  (4.19) 

Since  Ae(t )  is  time- varying  and  depends  on  the  desired  trajectory,  the  functions  m(t)  and 
M{t)  will  have  to  be  determined  numerically  after  solving  for  Z(t).  To  show  that  the 
closed-loop  system  is  locally  exponentially  stable,  we  want  to  show  that  the  time  deriva¬ 
tive  of  the  candidate  Lyapunov  function  (4.18)  is  negative  definite  in  a  neighborhood  of 
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the  origin.  We  compute  the  time  derivative  as 


V  =  qj  Zqe  +  qj  Zqe  +  qj  Zqe 

=  —  qj  [AjZ  +  ZAe  +  Q-Z  ( BeBj  -  r2DeDj)  Z]  qe 

+  qj  (Ae  -  KBeBjZ)T  Zqe  +  qJZ  (Ae  -  kBcBJZ)  qe  +  2qJZo(\qe\ ) 

=  -qj  [Q  +  (2k  -  1  )ZBeBjZ  +  r2ZDeDjZ]  qe  +  2qJZo(|qe|).  (4.20) 


If  we  choose  K,(t)  >  \  on  [t0,tf],  then  for  all  t  €  [to,  t}\  we  have 

[Q  +  (2k  -  1  )ZBeBjZ  +  j~2ZDeDjZ]  >  0. 


The  remaining  term  in  (4.20)  is  sign  indefinite,  but  vanishes  as  |qe|  approaches  the  origin, 
so  we  can  use  the  bounds  on  Z  and  our  knowledge  of  Q  to  find  a  neighborhood  of  the 
origin  where  V  <  0.  Following  the  approach  presented  by  [87,88],  and  using  routine 
arguments,  we  can  find  some  e  >  0  such  that 

[fe(t, qe)  -  Ae(t)qe]  <  |qe|,  V qe  such  that  |qe|  <  e.  (4.21) 

This  implies  that 

|2qJZo(M|  <  J?|q«|2-  (4.22) 


Combining  (4.22)  with  (4.20),  we  get 

V(t,qe)  <  -qTe[Q  +  (2K-l)ZBeBjZ  +  r2ZDeD?Z}qe  +  ^q\qe\2 
<  -9|qe|2  +  ^|qe|2  =  “9|qe|2 


Equation  (4.23)  leads  to  the  following  bounds  on  the  Lyapunov  function  and  the  magni¬ 
tude  of  the  state  vector: 


V(t,  qe)  <  V(t0-,qeo)  exp 


M*)l  <  M\/~exP 


■Ut 

2  M  . 

-Ut 

4  M 


(4.24) 

(4.25) 
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so  that  in  an  e  neighborhood  of  the  origin,  we  have  exponential  stability  for  the  closed- 
loop  system.  Simulation  results  show  that  these  convergence  bounds  for  the  system  are 
relatively  conservative. 

Several  remarks  about  the  control  law  are  now  in  order.  First,  we  have  not  taken 
advantage  of  the  term  [(2«  -  1  )ZBeBjZ  +  7 ~2ZDeDjZ]  in  our  stability  analysis.  This 
term  is  nonnegative  definite  for  k  >  |  and  will  improve  the  stability  of  the  design  as 
can  be  seen  in  the  inequalities  preceding  (4.23).  A  more  detailed  analysis  of  this  part 
of  the  Lyapunov  function  should  lead  to  a  tighter  convergence  bound  on  the  error.  In 
addition,  since  we  do  not  rely  on  this  term,  our  stability  analysis  would  hold  even  if  the 
term  j^ZDgDjZ  was  not  included  in  the  GRDE  (4.8)  when  we  solved  for  Z.  Removing 
the  7 ~2ZDeDjZ  term  from  the  GRDE  is  equivalent  to  setting  7  =  00,  in  which  case 
there  is  always  a  solution  to  (4.8)  since  we  require  Q  >  0  and  Qf  >  0  in  the  design. 

The  second  remark  is  that  even  though  we  have  ignored  the  disturbances  in  the  sta¬ 
bility  analysis,  we  expect  the  state  feedback  control  law  to  exhibit  reasonable  disturbance 
attenuation  properties.  The  reason  for  this  expectation  is  that  the  control  law  closely 
resembles  the  optimal' disturbance  attenuating  control  law  for  the  linearized  system  and 
uses  the  same  Z(t)  matrix.  If  we  choose  «(i)  =  1,  we  recover  the  optimal  disturbance 
attenuating  controller.  The  time- varying  Z(t)  matrix  is  the  key  element  of  the  design 
that  provides  the  disturbance  attenuation  properties. 

We  have  demonstrated  a  form  of  local  exponential  stability  for  the  nonlinear  system. 
Our  approach  does  not  match  the  standard  definition  for  exponential  stability  because 
we  consider  a  finite  time  interval  and  the  standard  approach  considers  all  time  greater 
than  a  given  initial  time  [71].  The  analysis  is  still  helpful  because  if  the  time  interval 
is  long  enough  and  the  vehicle  approaches  the  desired  trajectory  exponentially  over  the 
interval,  we  will  have  reasonable  tracking  in  finite  time.  One  related  concern  is  that  (4.25) 
indicates  that  extremely  large  values  for  M  could  lead  to  large  changes  in  qe  as  the  vehicle 
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approaches  the  desired  trajectory.  This  point  will  be  important  when  we  consider  the 
role  of  parameter  7  in  the  control  law. 

The  acceptable  range  for  k (t)  is  the  interval  [5, 00),  which  also  appears  as  the  allowable 
range  for  a  design  parameter  used  by  Walsh,  Tilbury,  Sastry,  Murray,  and  Laumond  [88]. 
The  similarity  is  not  a  coincidence  and  requires  a  brief  comment.  Walsh,  Tilbury,  Sastry, 
Murray,  and  Laumond  find  a  control  law  for  a  time- varying  nonlinear  system  through  lin¬ 
earization  and  by  exploiting  known  bounds  on  the  controllability  Gramian.  The  bounds 
on  the  controllability  Gramian  are  equivalent  to  the  existence  of  a  solution  to  a  Lyapunov 
equation.  If  we  remove  the  [(2k  -  l)ZBeBj Z  +  ^f~2ZDeDj Z]  term  from  (4.8),  as  we  did 
in  the  stability  proof,  we  have  a  Lyapunov  equation  for  the  linearized  system.  Including 
the  term  containing  Be  and  De  in  (4.8),  however,  allows  the  control  law  to  account  for 
how  the  inputs  and  disturbances  enter  the  system,  which  provides  additional  robustness 
properties. 

The  final  remark  is  that  since  we  are  using  a  control  law  based  on  the  linearization  of 
the  system,  we  cannot  draw  any  conclusions  about  the  disturbance  attenuation  properties 
of  the  closed-loop  nonlinear  system  on  a  global  scale.  To  make  a  statement  along  these 
lines,  we  would  have  to  find  a  value  function  to  satisfy  the  appropriate  Hamilton- Jacobi- 
Isaacs  inequality,  given  in  (4.13).  The  system  will  maintain  some  disturbance  rejection 
capabilities  based  on  the  choice  for  7,  and  we  explore  the  role  of  this  parameter  in  more 
detail  next. 


4.4  Disturbance  Attenuation  Analysis 

We  are  interested  in  how  well  the  control  law  attenuates  the  effects  of  disturbances 
in  the  system,  and  the  parameter  7  provides  one  measure  of  this  capability.  As  long 
as  we  maintain  7  >  7*,  smaller  values  for  7  indicate  better  disturbance  attenuation 
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properties.  Pan  and  Ba§ar  [86]  prove  that  the  controller  designed  using  the  linearization 
of  the  nonlinear  system  can  achieve  a  performance  level  7  if  the  difference  between  the 
nonlinear  system  and  its  linearization  is  sufficiently  small.  This  is  exactly  the  situation  in 
our  case  if  we  assume  that  the  initial  configuration  errors  are  small.  Instead  of  relying  on 
this  valuable  result  to  characterize  the  disturbance  attenuation  properties  of  the  system, 
this  section  describes  how  variations  in  7  influence  the  trajectory  of  the  vehicle. 

We  originally  thought  that  we  would  be  able  to  use  the  special  structure  in  the 
equations  of  motion  to  decompose  the  system  and  develop  more  precise  results,  but  this 
approach  is  not  possible  for  the  state  feedback  control  law.  We  can  easily  perform  the 
decomposition,  but  the  resulting  subsystems  are  coupled  and  each  depends  on  the  state  of 
the  other  subsystem.  Since  we  are  using  state  feedback,  the  associated  Riccati  equations 
must  be  solved  in  reverse  time,  starting  at  the  final  time.  If  we  could  solve  the  Riccati 
equations,  we  could  compute  the  control  inputs  for  the  system  and  calculate  the  states 
by  solving  the  system  differential  equations  in  forward  time.  We  cannot  execute  this 
approach  because  the  Riccati  equations  depend  on  the  future  system  matrices,  which  in 
turn  depend  on  the  future  error  states.  We  cannot  accurately  compute  the  future  error 
states  without  the  controller,  so  we  cannot  use  this  design  approach. 

We  note,  however,  that  we  will  use  the  above  decomposition  technique  to  successfully 
analyze  the  imperfect  state  measurement  controller  design  presented  in  Chapter  5.  The 
approach  works  for  the  imperfect  state  case  because  the  system  matrices  depend  on 
the  estimated  states.  The  differential  equations  to  calculate  the  estimated  states  are 
computed  in  forward  time,  so  we  can  use  the  state  estimate  in  the  control  law  and  do 
not  need  to  know  the  future  state  estimates  to  find  a  solution. 

To  provide  a  more  complete  description  of  the  disturbance  attenuation  properties  of 
the  state  feedback  controller,  we  will  take  a  different  approach.  Recall  that  the  differential 
equations  for  the  error  system  are  given  by  (4.5),  where  the  term  in  square  brackets  is 
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nonlinear  and  o(|qe|).  Equation  (4.5)  is  not  an  approximation,  because  it  accounts  for 
the  nonlinear  term.  We  make  an  approximation  only  when  we  drop  the  nonlinear  term 
and  use  the  linear  portion  to  solve  the  associated  GRDE.  Suppose  we  rewrite  (4.5)  as 

qe  =  Ae(t)  qe  +  Be(t)ue  +  De(t)  w,  qe(<o)  =  qeo  (4.26) 


where  we  define 


De(t) W  :=  De(t) w  +  [fe(t,  qe)  -  Ae(t) qe] . 

The  term  De(t)  represents  a  6  x  5  matrix  where  the  upper  five  rows  are  an  identity 
matrix  and  the  bottom  row  is  all  zeros.  We  would  then  select  w  as  a  vector  in  1R5  that 
accounts  for  the  original  disturbances  w  and  the  nonlinear  terms  in  (4.5).  We  note  that 
this  representation  is  again  an  exact  description  of  the  nonlinear  system.  We  have  only 
changed  the  nature  of  the  disturbance  term  to  make  the  system  appear  linear. 

We  can  now  design  a  state  feedback  control  law  for  this  new  representation  of  the 
system  by  following  the  approach  in  Section  4.2.  The  only  adjustment  we  will  have  to 
make  to  the  existing  design  is  to  change  the  De  matrix  to  De  in  GRDE  (4.8).  The 
new  design  will  be  more  conservative  than  the  old  design  because  we  are  accounting 
for  artificial  disturbances  that  appear  because  of  the  nonlinear  terms.  Relabeling  the 
disturbances  is  similar  to  a  step  used  by  Kang  [91]  to  develop  disturbance  attenuation 
results  for  a  fully  actuated  system.  This  new  approach  allows  us  to  precisely  state  the 
disturbance  attenuation  results  for  the  state  feedback  design  in  terms  of  De  and  w. 

Consider  the  linear-quadratic  zero-sum  differential  game  given  by  (4.26)  with  perfor¬ 
mance  index  (4.6)  and  with  the  closed-loop  perfect  state  information  structure.  Let  the 
system  be  defined  on  the  time  interval  [to,  t/].  Replace  De  by  De  in  the  GRDE  (4.8) 
and  in  (4.11)  throughout  the  problem.  Let  7*  be  defined  by  (4.9).  Then,  following  [76], 
for  7  >  7*,  the  differential  game  admits  a  unique  feedback  saddle-point  solution  which 
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is  given  by  (4.10)  and  (4.11),  where  Z7(-)  is  the  unique  solution  to  (4.8).  In  addition, 
the  minimax  attenuation  level  is  equal  to  7*.  For  a  particular  7  >  7*,  the  suboptimal 
controller  that  achieves  attenuation  level  7  is  given  by 

«}(*>  q  e(t))  =  -BT(t)Zj(t)qe(t).  (4.27) 

These  results  provide  a  mathematical  characterization  of  the  performance  of  the  perfect 
state  feedback  tracking  controller.  Along  with  this  rigorous  description  of  the  results,  we 
are  also  interested  in  the  qualitative  disturbance  attenuation  properties  of  the  system. 

As  we  allow  7  to  decrease  to  7*,  the  GRDE  (4.8)  approaches  an  equation  with  a  con¬ 
jugate  point  and  the  maximum  eigenvalue  of  Z(t)  increases  without  bound.  The  large 
eigenvalue  in  Z(t)  can  increase  the  control  effort  in  the  direction  associated  with  the 
eigenvalue,  which  has  two  impacts  on  the  system.  First,  the  control  law  becomes  very 
sensitive  to  errors  aligned  with  the  eigenvalue’s  associated  eigenvector.  This  sensitivity 
means  that  small  errors  in  this  direction  generate  large  control  inputs.  The  large  con¬ 
trol  inputs  may  overcompensate  for  the  errors  and  cause  oscillations  in  the  response  as 
these  errors  are  eliminated.  Second,  in  terms  of  the  trajectory  of  the  vehicle,  the  larger 
eigenvalue  can  cause  the  vehicle  to  approach  the  desired  path  more  quickly  because  of 
the  increased  control  effort. 

As  mentioned  in  the  previous  subsection,  increasing  the  value  of  7  decreases  the 
role  of  the  7 ~2ZDeDjZ  term  in  (4.8).  This  term  is  improving  the  convergence  rate 
for  the  system,  so  in  general,  increasing  7  will  cause  the  vehicle  to  converge  to  the 
desired  trajectory  more  slowly.  From  another  perspective,  increasing  7  moves  the  GRDE 
away  from  a  conjugate  point  condition,  which,  for  a  fixed  value  of  t,  tends  to  decrease 
the  maximum  eigenvalue  of  Z(t).  The  reduced  maximum  eigenvalues  indicate  that  the 
vehicle  will  require  more  time  to  track  the  desired  trajectory. 
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Ideally,  we  would  like  to  determine  the  value  of  7*  for  a  given  desired  trajectory,  so 
that  we  can  always  pick  7  >  7*.  For  the  problem  we  have  developed,  there  is  no  analytical 
approach  to  find  7*  because  of  the  time- varying  nature  of  the  system  equations  and  the 
matrix  Ae(t).  Even  though  we  are  working  with  the  linearized  system,  the  only  way  to 
find  7*  is  to  select  a  desired  trajectory  and  numerically  simulate  (4.8)  for  different  values 
of  7  to  search  for  the  smallest  value  that  provides  a  solution. 

By  recharacterizing  the  disturbance,  we  were  able  to  make  a  precise  statement  about 
the  disturbance  attenuation  properties  of  the  linearized  H°°  controller.  We  also  made 
several  qualitative  observations  about  the  relationship  between  the  disturbance  attenu¬ 
ation  parameter  7  and  the  performance  of  the  controller  in  the  closed-loop  system.  We 
mentioned  the  possibility  of  proving  additional  disturbance  attenuation  properties  by 
using  the  results  in  [86],  but  we  will  defer  a  more  complete  look  at  this  approach  until 
we  consider  the  imperfect  state  measurement  controller  described  in  Chapter  5. 

4.5  Nonminimum  Phase  Vehicle  Control 

The  nonlinear  model  of  an  underactuated  vehicle  we  have  analyzed  heretofore  is 
minimum  phase  with  respect  to  the  x-y  position  outputs.  We  will  demonstrate  that 
the  motion  planning  and  state  feedback  tracking  control  techniques  developed  with  this 
model  will  also  work  when  applied  to  a  nonminimum  phase  system.  We  recall  that  a 
nonlinear  system  becomes  nonminimum  phase  when  the  zero  dynamics  associated  with 
some  output  are  not  asymptotically  stable  [61].  To  create  a  nonlinear  model  with  the 
nonminimum  phase  property,  we  replace  Equations  (1.1)  through  (1.3)  with  the  following 
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dynamic  equations: 


mu  v  r  —  du  u  +  u\  +  wi 

(4.28) 

mvur  —  dvv  +  kvu2  +  w2 

(4.29) 

mruv  —  drr  +  u2  +  Wz ■ 

(4.30) 

The  kinematic  equations  for  the  new  model  are  the  same  as  (1.4)  through  (1.6).  In  (4.29), 
the  parameter  kv  is  a  coupling  coefficient  which  relates  the  pure  torque  input  u2  and  the 
lateral  acceleration  of  the  vehicle.  This  lateral  force  is  a  natural  addition  to  a  ship  model 
when  the  thrust  and  rudder  are  located  at  the  trailing  end  of  the  vessel.  Turning  the 
rudder  causes  a  rotation  as  well  as  a  slight  lateral  force  on  the  ship.  We  should  note  that 
the  dynamic  equations  for  the  original  model  described  in  Section  1.4  can  be  recovered 
from  this  nonminimum  phase  model  by  setting  kv  to  zero. 

If  we  write  the  nonminimum  phase  equations  of  motion  in  compact  form,  we  get 

q  =  /(f,q)+Bnu  +  Dw  (4.31) 

where  Bn  is  the  new  input  matrix  that  accounts  for  the  coupling  between  the  rotational 
and  lateral  acceleration  inputs.  Note  that  the  nonlinear  function  f(t,  q)  and  the  distur¬ 
bance  terms  do  not  require  any  changes  for  the  new  system.  Since  there  are  no  changes  to 
the  general  structure  of  the  system,  we  can  directly  apply  the  controller  design  developed 
in  Section  4.2  to  the  new  system. 

A  close  inspection  of  the  basic  motion  planning  algorithm  in  Section  3.2  shows  that 
we  do  not  need  to  make  any  changes  to  it  to  handle  the  nonminimum  phase  case.  We  do 
have  the  option  of  changing  how  we  estimate  the  control  input  u2,  because  it  now  appears 
in  two  of  the  equations,  but  it  makes  sense  to  maintain  the  existing  approach.  The  change 
would  be  to  use  Equation  (4.29)  to  estimate  u2  instead  of  using  Equation  (4.30).  Since 
typically  both  v  and  kv  are  small,  errors  in  estimating  v  would  translate  into  larger  errors 
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in  the  estimate  of  u2  using  (4.29)  than  if  we  use  (4.30).  If  the  coupling  coefficient  kv  was 
large,  then  we  could  use  either  (4.29)  or  (4.30)  to  estimate  u2  with  approximately  the 
same  amount  of  error.  The  motion  planning  algorithm  will  account  for  the  nonminimum 
phase  behavior  in  the  system  through  the  iterated  //'“-filter,  which  relies  on  Bn.  In 
summary,  to  adapt  the  motion  planning  algorithm  for  this  nonminimum  phase  nonlinear 
system,  we  just  need  to  adjust  the  input  matrix  in  the  //“-filtering  equations. 

The  nonlinear  function  /(/,  q)  has  not  changed,  so  there  is  also  no  change  in  the  lin¬ 
earization  of  the  error  system,  except  for  the  input  matrix  Bn.  The  system  has  maintained 
the  same  general  structure  and  we  can  use  the  linearized  system  to  find  the  //“-optimal 
controller.  We  solved  for  the  linearized  controller,  applied  it  to  the  nonlinear  system, 
and  simulated  the  closed-loop  behavior  of  the  model.  Chapter  6  reports  on  the  positive 
results  of  using  the  state  feedback  tracking  control  algorithm  with  a  nonminimum  phase 
system. 

The  tracking  controller  works  for  both  the  minimum  and  nonminimum  phase  nonlin¬ 
ear  systems  because  the  H°°  controller  is  inherently  robust  and  the  differences  between 
the  two  systems  are  relatively  small.  In  addition,  the  designs  fully  account  for  the  new 
input  matrix,  which  captures  the  nonminimum  phase  behavior.  If  the  coupling  between 
the  rotational  and  lateral  accelerations  was  very  large  or  if  there  was  a  different  type  of 
nonminimum  phase  behavior,  then  we  might  expect  a  decrease  in  the  performance  of  the 
tracking  controller. 

From  this  brief  analysis  of  a  single  system,  we  can  project  that  the  motion  planning 
and  state  feedback  tracking  controller  algorithms  can  accommodate  nonminimum  phase 
systems.  The  details  of  how  we  implement  each  algorithm  will  have  to  be  tailored  for 
each  new  system,  but  the  general  approach  and  the  overall  results  should  remain  the 
same. 
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4.6  Region  of  Attraction  Analysis 


Our  final  investigation  in  this  chapter  considers  the  region  of  attraction  for  the  closed- 
loop  system  with  state  feedback  tracking  control.  The  region  of  attraction  is  the  set  of 
initial  conditions  from  which  the  tracking  controller  can  cause  the  vehicle  to  approach  the 
desired  final  configuration  within  the  specified  time  interval.  We  have  two  methods  for 
estimating  the  region  of  attraction  for  the  nonlinear  model  of  an  underactuated  vehicle. 
The  first  method  uses  the  proof  of  local  exponential  stability  presented  in  Section  4.3  to 
develop  an  analytical  description  of  the  region  of  attraction.  The  second  approach  uses 
numerical  simulation  to  search  a  range  of  initial  conditions  to  estimate  the  region.  The 
following  two  subsections  describe  each  approach  and  the  results. 

4.6.1  Analytical  description 

One  way  to  make  a  definitive  statement  about  the  size  of  the  region  of  attraction  for 
a  nonlinear  system  is  to  find  a  Lyapunov  function  for  the  system  and  a  corresponding 
region  where  the  derivative  of  the  Lyapunov  function  is  negative  definite.  We  can  then 
estimate  the  region  of  attraction  as  the  largest  level  set  of  the  Lyapunov  function  that 
is  within  the  region  where  the  function  has  a  negative  derivative.  This  estimate  for 
the  region  of  attraction  is  a  lower  bound  for  the  actual  region  and  may  be  extremely 
conservative.  This  approach  may  not  fully  describe  the  shape  of  the  region,  but  it  does 
provide  a  direct  method  for  developing  analytical  results.  We  will  present  the  key  steps 
required  to  find  a  lower  bound  on  the  region  of  attraction  for  the  underactuated  system. 

We  will  first  identify  the  set  of  configurations  that  guarantee  that  the  Lyapunov 
function  has  a  negative  derivative.  From  Section  4.3,  the  key  inequality  that  must  be 
satisfied  to  obtain  local  exponential  stability  is  given  by  Equation  (4.22),  which  we  rewrite 


78 


here  with  slightly  different  notation: 


\2qJ Z[fe(t,qe)  ~Aeqe\\  <  ^q  |qe|  (4.32) 

where  Z  is  the  solution  of  the  generalized  Riccati  differential  equation  for  the  linearized 
system  and  the  matrix  Q  =  ql  is  part  of  the  performance  index.  Recall  that  we  are 
working  with  the  error  equations,  which  are  denoted  with  an  e  subscript.  Our  goal  is  to 
find  a  value  of  e  such  that  qe  satisfies  (4.32)  whenever  |qe|  <  e. 

To  perform  the  analysis,  we  require  bounds  on  the  positive  definite  matrix  Z(t),  in 
the  form 


0  <  ml  <  Z{t)  <  MI.  (4.33) 

The  generalized  Riccati  differential  equation  used  to  find  Z(t)  depends  on  the  matrix 
Ae(t),  which  in  turn  depends  on  the  desired  trajectory  the  system  is  trying  to  track.  We 
cannot  place  a  meaningful  bound  on  Ae(t)  without  restricting  the  desired  trajectories  for 
the  system.  If  we  did  restrict  the  set  of  desired  trajectories  to  get  a  bound  on  Ae(t),  the 
set  would  likely  be  too  small  to  account  for  many  reasonable  motions.  As  a  compromise, 
to  complete  the  analytical  description  of  the  region  of  attraction,  we  will  have  to  resort  to 
a  numerical  estimate  for  the  bounds  on  Z(t)  given  by  (4.33).  Given  a  desired  trajectory, 
we  can  compute  Z(t)  and  then  solve  for  M  and  m  as,  respectively,  the  largest  and 
smallest  eigenvalues  of  Z(t)  for  all  t  G  [t0,tf].  We  will  use  the  estimated  bounds  on  Z(t ) 
to  describe  the  region  of  attraction  for  the  specified  desired  trajectory.  We  will  have  to 
find  new  values  for  M  and  m  for  each  desired  trajectory  of  interest. 

To  convert  the  inequality  (4.32)  into  a  bound  on  the  initial  condition  for  the  system, 
we  used  the  Zi  variables  to  represent  the  position  and  orientation  of  the  vehicle  instead  of 
the  x,  y,  and  variables.  The  Zi  variables  simplify  the  equations  of  motion  and  allow  us 
to  perform  some  cancellations  to  reach  a  compact  expression  for  the  region  of  attraction. 
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Using  (4.33),  we  can  show  that  (4.32)  is  equivalent  to 


|qj  [fe(t,  qe)  -  Aeqe]  |  <  |qe|2  • 


(4.34) 


With  the  Zi  variables,  we  have 


[fe(t,  qe)  -  Aeqe  = 


iT 


muvere  mvuere  mrueve  z2ere  -zlere  0 


where  mu ,  rnv,  and  mr  are  the  mass  parameters  in  the  equations  of  motion.  Expanding 
both  sides  of  (4.34),  we  get 

| (mu  +  mv  +  mr)uevere \  <  ^  \u2e  +  v2e  +  r2e  +  zje  +  z\e  +  z\e I 


4  M 


which  leads  to 


\uPverJ  < 


ul  +  v2e+r]  +  z\e  +  4s  +  zl 


(4.35) 


4 M  \mu  +  mv  +mr  \ 

Suppose  we  restrict  |qe|  <  e  <  1,  for  some  e  >  0,  and  then  we  maximize  the  left-hand 
side  of  (4.35)  subject  to  |qe|  <  £.  The  maximum  occurs  when  ue  =  ve  =  re  =  Using 
the  constraint  on  qe,  we  also  have  |qe|2  <  e2,  so  we  can  rewrite  (4.35)  as 


< 


qe 


3a/3  4 M  |  mu  +  mv  +  mr 


which  simplifies  to 


e  < 


(4.36) 


4 M  \mu  +  mv  +  mr  \ 

We  now  set  e  equal  to  the  right-hand  side  of  (4.36),  because  it  is  the  largest  value  that 
allows  us  to  claim  that  qe  satisfies  (4.34)  whenever  |qe|  <  e.  Therefore,  the  region  defined 
by 


|qe|  < 


3g\/3 


4  M  |  mu  +  mv  +  mr 


—  •  Qmax 


(4.37) 
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is  a  conservative  estimate  for  the  region  of  initial  conditions  that  guarantee  that  the 
Lyapunov  function  will  maintain  a  negative  derivative  along  the  associated  trajectory. 
We  will  now  find  a  level  set  for  the  Lyapunov  function  within  this  region. 

A  level  set  is  the  collection  of  configurations  such  that  we  can  place  an  upper  bound 
on  the  value  of  the  Lyapunov  function.  We  recall  that  the  Lyapunov  function  is  V (qe)  = 
qj  Zqe  and  define  the  level  set  as 

nc  :=  {qe  6  R6  I  V(qe)  <  c}  (4.38) 


where  c  is  a  parameter  describing  the  level.  Using  the  lower  bound  in  (4.33)  and  (4.37) 
and  following  the  standard  arguments  in  [71],  we  can  select 


c  <  min  U(qe) 

|cje  |  —  9max 


27  mq2 

16M2  |  mu  +  mv  +  mr 


With  this  value  for  c,  the  region  defined  by  Qc  provides  an  estimate  for  the  region  of 
attraction  for  the  system  and  describes  the  set  of  initial  conditions  that  will  generate 
trajectories  approaching  the  goal  configuration  at  the  required  time. 

We  performed  three  simulations  of  typical  motions  to  determine  bounds  on  the  matrix 
Z(t)  and  found  that  choosing  m  =  0.2  and  M  =  20  would  be  sufficient  in  all  three  cases. 
We  also  chose  q  =  1  and  picked  values  for  mu,  mv,  and  mr  such  that  \mu  +  mv  +  mr\  =  1. 
These  desired  trajectories  and  parameter  choices  result  in  a  numerical  value  of  e  =  0.065 
and  c  =  0.0008.  These  values  for  s  and  c  indicate  that  the  initial  error  magnitude  must 
be  very  small  to  guarantee  that  the  tracking  controller  will  cause  the  vehicle  to  approach 
the  desired  final  configuration.  The  estimate  is  extremely  conservative  because  we  are 
using  the  worst-case  bound  on  the  matrix  Z(t).  As  the  vehicle  moves,  the  upper  bound 
on  Z(t )  will  often  be  less  than  MI,  so  using  the  maximum  upper  bound  contributes  to 
the  conservative  nature  of  the  estimate.  To  find  a  more  accurate  estimate  for  the  region 
of  attraction,  we  will  use  a  numerical  approach. 
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4.6.2  Numerical  description 

The  numerical  approach  for  estimating  the  region  of  attraction  is  a  brute  force  tech¬ 
nique  that  scans  a  set  of  initial  conditions  and  determines  which  of  them  lead  to  tra¬ 
jectories  that  approach  the  desired  final  configuration.  To  reduce  the  dimension  of  the 
search  space,  we  assumed  the  desired  trajectory  generated  by  the  motion  planning  algo¬ 
rithm  had  no  initial  position  error.  This  assumption  is  valid  when  we  use  the  version  of 
the  motion  planning  algorithm  described  in  Subsection  3.2.3  that  generates  the  desired 
trajectory  starting  from  the  estimated  initial  conditions.  With  no  error  in  the  position 
variables,  we  only  have  to  vary  the  remaining  four  states  to  characterize  the  region  of 
attraction,  which  significantly  reduces  the  computational  requirements. 

We  applied  the  motion  planning  algorithm  starting  at  q  =  [1,  0,  0,  0,  0,  0]T  and  mov¬ 
ing  to  [1,  0,  0,  8,  8,  0]T,  to  generate  the  desired  trajectory  for  the  first  set  of  simulations. 
Using  this  desired  trajectory,  we  computed  Z(t)  for  the  entire  time  interval.  We  then 
selected  an  initial  condition  and  applied  the  state  feedback  control  law  to  simulate  the 
motion  of  the  vehicle.  With  this  desired  trajectory,  we  performed  three  simulations  with 
different  sets  of  initial  conditions.  The  initial  conditions  were 

1.  ue(t0),  ve(t0),  re(t0)  e  [-2,  -1.5,  -1.0,  -0.5,  0.0,  0.5,  1.0,  1.5,  2.0], 

^e(to)  e  [-7T,  -371-/4,  -tt/2,  -tt/4,  0,  tt/4,  tt/2,  3tt/4,  7 r]. 

2.  ue(to),  ve(to),  re(t0 )  G  [-1,  -0.8,  -0.6,  -0.4,  -0.2,  0.0,  0.2,  0.4,  0.6,  0.8,  1.0], 
ipe(to )  G  [-7T,  —471-/5,  -37T/5,  — 27t/5,  -7T/5,  0,  7t/5,  27T/5,  371-/5,  471-/5,  7 r]. 

3.  ue(t0),  ve(t0),  re{t0)  e  [-0.5,  -0.4,  -0.3,  -0.2,  -0.1,  0.0,  0.1,  0.2,  0.3,  0.4,  0.5], 
ipe(t0)  e  [-71-/2,  -2tt/5,  -3tt/10,  — tt/5,  — tt/10,  0,  tt/10,  tt/5,  3tt/10,  2tt/5,  tt/2]. 

The  first  two  sets  of  initial  conditions  led  to  trajectories  that  did  not  converge  to  the 
desired  final  configuration.  However,  in  the  third  set,  all  of  the  initial  conditions  generated 


82 


trajectories  that  approached  the  desired  configuration.  We  can  thus  conclude  that  for  this 
desired  trajectory  and  within  the  resolution  of  this  numerical  computation,  the  region  of 
attraction  is  between  the  second  and  third  sets  of  initial  conditions.  We  can  therefore 
use  the  third  set  of  initial  conditions  to  form  a  conservative  estimate  for  the  region  of 
attraction.  Although  this  estimate  is  conservative,  it  is  an  order  of  magnitude  larger  than 
the  analytical  estimate  for  the  region  of  attraction  developed  in  Subsection  4.6.1. 

To  further  support  these  results,  we  checked  the  third  set  of  initial  conditions  given 
above  with  the  two  other  desired  trajectories  considered  in  the  analytical  estimate  for  the 
region  of  attraction.  In  both  cases,  all  of  the  initial  conditions  in  the  third  set  generated 
trajectories  that  converged  to  the  desired  final  configuration. 

The  numerical  approach  for  estimating  the  region  of  attraction  is  inherently  limited 
because  we  can  only  survey  a  small  number  of  initial  conditions  and  desired  trajectories 
within  a  reasonable  amount  of  time.  Improving  the  resolution  of  the  description  of  the 
region  of  attraction  or  finding  its  exact  boundary  are  computationally  expensive  tasks 
that  may  not  be  worth  the  effort.  The  region  of  attraction  described  by  the  third  set  of 
initial  conditions  is  reasonably  large  for  the  types  of  simulations  we  have  considered  and 
indicates  that  the  state  feedback  tracking  controller  provides  a  practical  solution  to  the 
nonlinear  under  actuated  vehicle  control  problem  in  many  situations. 

4.7  Perfect  State  Feedback  Summary 

The  perfect  state  feedback  controller  design  provides  the  initial  approach  for  making 
an  underactuated  vehicle  track  a  feasible  desired  trajectory  while  rejecting  the  effect  of 
disturbances.  The  approach  relies  on  linearization  and  H 00  design  techniques  to  develop 
the  controller.  We  have  demonstrated  that  the  closed-loop  system  achieves  local  expo¬ 
nential  stability,  and  we  have  characterized  the  disturbance  attenuation  properties  of  the 
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system.  In  addition,  we  have  explained  how  the  approach  can  apply  to  nonminimum 
phase  systems  and  used  two  methods  to  estimate  the  region  of  attraction  for  the  closed- 
loop  system.  As  will  be  illustrated  in  Chapter  6,  the  approach  provides  exceptionally 
good  tracking  performance  when  the  full  state  is  available  for  feedback.  When  the  model 
does  not  allow  us  to  access  the  entire  state  for  feedback,  we  need  another  approach  to 
develop  the  controller.  We  will  address  this  imperfect  state  measurement  case  in  the  next 
chapter  (Chapter  5)  by  using  a  state  estimate  in  the  state  feedback  control  law. 
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CHAPTER  5 


IMPERFECT  STATE  MEASUREMENT 
TRACKING  CONTROL 


The  controller  developed  in  Chapter  4  relies  on  perfect  state  measurements  for  the 
feedback  law.  In  a  realistic  situation,  the  entire  state  for  the  system  cannot  generally  be 
measured.  In  our  example  of  an  underactuated  ship,  we  can  expect  to  directly  measure 
the  position  and  orientation  of  the  vehicle,  but  not  the  velocities.  In  addition,  the 
position  and  orientation  measurements  will  likely  be  corrupted  by  disturbances,  which 
we  will  have  to  account  for  in  the  analysis.  We  will  now  modify  the  controller  designed 
in  the  previous  chapter  to  handle  the  imperfect  state  measurement  case.  We  still  rely  on 
the  H°°  design  tools  to  obtain  a  controller,  and  will  exploit  an  existing  unique  structure 
in  the  model  of  the  underactuated  vehicle  to  achieve  the  desired  results.  The  approach 
will  allow  us  to  prove  a  modified  version  of  disturbance  attenuation  for  the  closed-loop 
system. 

5.1  Problem  Formulation 

The  problem  formulation  for  this  chapter  is  similar  to  that  for  Chapter  4.  As  before, 
we  want  to  determine  the  control  inputs  to  cause  the  underactuated  vehicle  to  track  a 
feasible  trajectory  and  attenuate  the  effect  of  disturbances.  The  control  law  will  have 
access  only  to  imperfect  measurements  of  the  position  and  orientation  states.  The  feasible 
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trajectory  will  be  constructed  in  advance  and  the  controller  will  be  able  to  use  the  inputs 
that  generated  the  trajectory. 

We  will  use  the  same  equations  for  the  actual,  desired,  and  error  systems  as  presented 
in  the  previous  chapter  in  (4.1)  through  (4.3).  We  note  that  the  error  equation  (4.3)  has 
an  equilibrium  point  at  the  origin  when  there  are  no  disturbances  present.  We  introduce 
the  measurement  equation 

y  (t)  =  C(t)q(t)  +  E(t)w(t)  (5.1) 

where  y  €  JR9  and  C(t)  and  E(t)  are  matrices  of  the  appropriate  dimensions.  Our 
approach  will  be  to  use  the  measurements  y (t)  to  estimate  the  full  state  of  the  system 
q (t)  and  then  substitute  the  estimate  for  the  actual  state  in  the  feedback  controller 
developed  in  Equation  (4.12).  We  will  denote  the  estimate  for  the  state  as  q. 


5.2  H00- Filter  Design  Using  System  Decomposition 


The  first  step  in  this  design  process  is  to  use  the  structure  of  the  model  for  the 
underactuated  vehicle  to  write  the  equations  of  motion  in  a  more  convenient  format.  The 
nonlinear  error  equations  for  the  vehicle  can  be  decomposed  into  two  sets  of  equations, 
where  each  equation  is  affine  if  the  state  of  the  other  subsystem  is  known.  For  this 
analysis,  we  will  suppress  the  e  subscript  on  the  error  equations.  The  equations  for  the 
two  subsystems  are  given  as 


qi  —  An(q2)qi  +  o;1(q2)  +  BiUX  +  DiWi 
q2  =  ^22(<4i)q2  +  G!2(qi)  +  B2U2  +  D2W2 

where 


- 

■ 

- 

qi 

,  u  = 

Ui 

,  w  = 

Wi 

q2 

U2 

w2 

(5.2) 

(5.3) 


(5.4) 
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For  our  model,  we  have 


- 

T 

r  1 

qi  = 

u  v  x  y 

,  02  = 

- 1 

-s- 

Ui  =  ui,  u2  =  u2 


T 


Wi 


W 1  w2 


w2  —  wz. 


The  Q'j(qj)  terms  are  nonlinear  and  we  would  like  to  identify  a  first-order  approximation 
for  each  term  to  further  simplify  (5.2)  and  (5.3).  To  do  so,  we  take  the  Jacobian  of  the 
oti(qj )  terms  with  respect  to  q j  to  identify  the  linear  portions 


•4l2  — 


dai{q2) 


dq2 


A 


21 


dMoi) 


q2=o 


dqi 


qi=0 


We  now  rewrite  the  subsystem  equations  as 


qi  —  ^4n(q2)Oi  +  ^4i2q2  +  [oi(q2)  —  ^i2q2]  +  -B1U1  +  Awi  (5-5) 

q2  =  A22{qx)q2  +  A2\qi +  [a.2(qi)  —  A2\q\\ -\- B2\i2  +  D2vf2.  (5.6) 

Combining  the  subsystem  equations,  we  arrive  at 


qi 

4n(q2)  Ai2 

Oi 

+ 

01(02)  -  ^1202 

q  = 

02 

A2i  ^22(01) 

02 

02(01)  --42101 

+ 

which  we  write  compactly  as 

q  =  A(q)q  +  a(q)  +  Bu  +  Bw.  (5.7) 

We  note  for  future  reference  that  o;(0)  =  0  in  the  systems  under  consideration.  This 
property  indicates  that  the  transformations  have  not  disturbed  the  equilibrium  point  at 


Bl  0 
0  B2 


Ul 

u2 
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Dl  0 
0  D2 


Wi 

w2 
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the  origin  for  the  error  equations.  We  are  going  to  estimate  the  state  q  using  y  so  we 
rewrite  (5.7)  as 

q  =  ;4(q)q  +  a(q)  +  Bu  +  Dw  +  [4(q)q  -  A(q)q  +  o(q)  -  a(q)]  (5.8) 

where  the  estimate  q  depends  on  past  measurements  yyt0,t]  :=  {y(r)  :  r  €  [to,*]}- 

We  can  consider  (5.8)  as  an  affine  system  with  time-varying  elements  and  a  nonlinear 
perturbation  term  in  square  brackets.  We  would  like  to  derive  results  similar  to  those  by 
Pan  and  Ba§ar  [86]  to  establish  attenuation  of  disturbances  for  this  system.  To  mimic 
the  problem  presented  in  [86],  we  rewrite  (5.8)  as 

q  =  A(q)q  +  a(q)  +  Bn  +  Dw  +  e  [a(t,  q)  +  b(t,  q)u  +  d(t,  q)w] .  (5.9) 

Equation  (5.9)  is  similar  to  Equation  (la)  in  [86],  with  the  difference  being  that  (5.9) 
contains  the  affine  term  a(q).  We  can  develop  a  disturbance  attenuating  controller  with 
the  affine  term  in  the  equations  if  we  make  one  assumption  and  minor  modifications  to 
the  derivation  in  [86].  Section  5.3  presents  the  controller  design  and  outlines  a  proof  of 
disturbance  attenuation  for  the  closed-loop  system. 

5.3  Disturbance  Attenuating  Controller 

Our  objective  is  to  find  a  control  law  based  on  the  imperfect  state  measurements 
in  (5.1)  that  achieves  disturbance  attenuation  level  7  for  the  nonlinear  system  (5.7). 
If  (5.9)  did  not  have  the  affine  term,  we  could  apply  the  results  in  [86]  almost  directly  to 
get  the  desired  performance  level.  The  affine  term  in  (5.9)  creates  one  difficulty  that  must 
be  addressed  to  reach  a  meaningful  conclusion  about  disturbance  attenuation.  Due  to  the 
presence  of  the  affine  term,  we  cannot  prove  the  standard  disturbance  attenuation  results, 
but  we  can  find  a  finite  bound  for  the  upper  value  of  the  associated  soft-constrained 
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dynamic  game.  This  upper  bound  allows  us  to  prove  a  modified  form  of  disturbance 
attenuation.  We  now  present  the  controller  equations  that  solve  this  problem  and  describe 
how  they  achieve  a  specified  level  of  disturbance  attenuation. 

We  form  the  candidate  control  law  by  replacing  the  actual  state  in  Equation  (4.10) 
with  the  estimate  for  the  state  q  to  get 

^(t,ylh,,])  =  -BT(t)zm  (5.10) 

where  q  is  generated  by 

q={A-  IBB1  -  ■i  '1DD')Z\q  +  a(q)  +  [J  -  'y-2EZ]-1ECTjV“1(y  -  Cq),  q(io)  =  0 

(5.11) 

with  E  satisfying  the  following  i7°°-filter  error  covariance  equation: 

E  =  AT,  +  EAt  -  E(C'T1V-1C'  -  7-2Q)E  +  DDT,  E(t0)  =  [Qo  ~  r)I]~l  (5.12) 

where  Q(t)  >  0,  Qo  -  r}I  >  0,  77  >  0,  and  N(t)  :=  E(t)ET(t)  >  0.  The  performance 
index  associated  with  the  imperfect  state  measurement  case  is 

L(u,w,q0;e)  =  [  [|q(t)|J  +  |u(t)|2  +  £  (q(t,  q(t))  +  uT(t)r(t,  q(t))u(t))]  dt 

J  to 

+|q(*/)IJ,-  (5-i3) 

We  will  assume  that  the  initial  state  is  unknown  and  treat  it  as  part  of  the  disturbance. 
The  associated  zero-sum  differential  game  has  the  following  soft-constrained  cost  func¬ 
tion: 

L7(u,w,q0;e)  =  L(u,w,q0;e:)  -  72  (|<]o|q0  +  ||w||2) .  (5.14) 

We  apply  control  law  (5.10)  to  (5.9)  and  to  the  performance  index  (5.14),  and  obtain 
equations  similar  to  Equations  (37)  and  (38)  in  [86].  For  brevity,  we  repeat  the  short 
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versions  of  these  two  equations  here: 


q  =  F(t)q  +  0(t)  +  G(t)w  +  £  [f{t,  q)  +  g(t,  q)w] ,  q(t0)  = 


x0 

0 


L 


* 

7 


“72qo  Qoqo  +  qT(tf)Qfq(tf) 

+  f  [qTH(t)q  -  72wtw  +  eh(t,  q)]  dt 
Jt0 


(5.15) 

(5.16) 


where  q  :=  [qT>  q  T]T.  In  (5.9)  the  A(q)  and  a(q)  terms  depend  on  q,  which  implies 
that  the  terms  F(t)  and  /3(t)  in  (5.15)  depend  on  q.  We  observe  that  q  depends  on  y, 
which  in  turn  depends  on  w,  so  w  enters  into  the  expression  for  q  through  F(t)  and 
We  will  shortly  perform  a  maximization  with  respect  to  w  and  q0,  which  will  have  to 
account  for  this  relationship.  We  make  the  following  assumption  to  address  this  aspect 
of  the  problem. 


Assumption  5.1  (A5.1)  The  effect  of  the  disturbance  term  w  on  the  terms  F(t)  and 
l 3{t )  is  small  enough  that  the  upper  value  of  the  performance  index  L*  remains  bounded. 

Our  concern  with  the  effect  of  w  on  q  stems  from  the  performance  index  (5.16), 
where  q  makes  a  positive  contribution  to  the  cost.  For  a  fixed  value  of  7,  if  w  has  a  large 
influence  on  q,  it  would  be  possible  for  the  disturbance  to  choose  a  value  to  make  the 
integrand  in  the  cost  function  arbitrarily  large.  On  the  other  hand,  if  w  has  a  negligible 
effect  on  q,  then  the  -72wTw  term  will  dominate  any  positive  contribution  w  makes 
through  qTH(t)q,  and  the  upper  value  of  L*f  will  remain  bounded. 

Assumption  A5.1  is  reasonable  if  a  large  change  in  the  disturbance  does  not  cause  a 
significant  change  in  the  values  of  F(t)  and  f3(t).  In  other  words,  F(t)  and  f3(t)  should 
be  relatively  insensitive  to  w.  If  this  is  the  case,  then  a  large  value  of  w  will  affect 
q  primarily  through  the  G(t) w  term  and  not  the  F(t)  or  fl(t)  terms.  (Note  that  the 
optimization  problem  will  account  for  the  role  of  G(t) w  through  a  Riccati  differential 
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equation  to  be  presented  shortly.)  Equivalently,  if  the  role  of  w  is  small  enough  that  it 
can  be  modeled  as  part  of  the  e  disturbance  in  square  brackets  in  (5.15),  then  we  can 
find  a  value  for  7  to  keep  the  upper  value  of  the  performance  index  finite.  To  further 
support  this  assumption,  the  simulation  results  in  Chapter  6  will  demonstrate  for  the 
model  system  that  changes  in  w  have  a  small  effect  on  F(t )  and  f3(t)  and,  hence,  there 
still  exists  a  finite  upper  bound  for  performance  index  L*. 

With  Assumption  A5.1  in  place,  we  now  want  to  find  an  upper  bound  for  the  value 
function  of  the  maximization  problem 

sup  sup  L*(w,  q0)  (5.17) 

q06lRn  vreHw 

subject  to  the  constraint  (5.15)  and  with  7iw  representing  the  class  of  admissible  distur¬ 
bances.1  We  start  by  separating  the  supremum  operations  to  get 

sup  sup  L*(w,q0)  <  sup  < -72qoQoqo 
q0 eiRn  w enw  qo eiRn  l 

+  sup  |qT(t/)Q/q(t/)+  [  [qTi7 (t)q  -  72wTw  +  eh(t, q)]  dt\X.  (5.18) 
'WZz'M.w  J  to  ' 

Following  Pan  and  Ba§ar  [86],  we  focus  on  the  inner  maximization  and  define 
Z7(w,q0):=  sup  |qT(t/)Q/q(t/)  +  /  [qTtf(t)q  -  72wTw  +  eh(t,  q)]  dt \  .  (5.19) 

WG'Kixj  L  v  to  ' 

If  e  =  0,  this  problem  admits  a  maximum  qT(t0)H(to)q(to)  +  CT(^o)q(^o)  +  ^(to)  where 
E(t)  is  the  nonnegative  definite  solution  to  the  following  GRDE: 

S  +  FtE  +  EF  +  j-2~GGtZ  +  H  =  0,  S(tf)  =  Qf  (5.20) 

and  C(t)  and  m(t)  satisfy  the  following  differential  equations: 

(  +  2Z(3  +  Ft  C  +  r2ZGGT(  =  0,  C  (tf)  =  0  (5.21) 

m  +  CT/3+iijCTGGTC  =  0.  m(t,)  =  0.  (5.22) 

1Here,  Hw  is  the  Hilbert  space  of  square-integrable  functions  on  [t0,  tf],  taking  values  in  Rv . 
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These  last  two  equations  are  a  result  of  the  affine  term  /?  in  the  constraint  equation 
for  the  maximization  problem.  To  demonstrate  a  disturbance  attenuation  level  of  7,  we 
follow  the  proof  in  [86]  and  consider  the  following  GRDE  when  e  >  0: 

S,  +  FrZs  +  ESF  +  j-2EsGGtEs  +  H  +  SI  =  0,  Es(tf)  =  Qf  (5.23) 


where  S  is  a  small  positive  scalar.  Equation  (5.23)  has  a  solution  for  sufficiently  small 
S  >  0.  Let  (g  and  mg  be  the  solutions  to  (5.21)  and  (5.22),  respectively,  with  E  replaced 
by  Eg.  Partition  E{  and  (g  as 


Eg(t)  = 

Egn(t) 

“<512(0 

.  0(0  = 

(<51  (0 

Eg21  ( t ) 

£<522(0 

(<52(0 

Skipping  some  of  the  details  in  the  paper  [86] ,  we  can  show  that 

Egu(t0)  <  72E_1(t0)  +  7 V  =  72Qo-  (5.24) 


For  a  fixed  5  >  0,  the  function  qTEg(t)q  is  an  upper  bound  for  qTE(t)q  in  the  maximiza¬ 
tion  problem  (5.17).  This  result  implies 

sup  {qT(tf)Qfq(tf)  +  [  [qTif(t)q-72wTw  +  eh(t,q)]  dt 
v  to 

<  qT {to)Eg(t0)q{to)  -|-  (^J (t0)q(t0)  +  mg(t0).  (5.25) 

We  now  consider  the  complete  maximization  problem  in  (5.18)  to  get 


sup  sup  L*  < 
q0e2Rn  w enw 


sup  {-72qoQoqo  +  qos<s(io)qo  +  C7(*o)qo  +  ^<5(^0)} 


qo<ElRn 


sup  {-72qjQoqo  +  q?s<5i1(t0)qo  +  (7i(^o)qo  +  mg(t0)} 
qo  elRn 

tC^l(^o)  [y2Qo  -  “<511  (^o)]  1  Csi(U)  +  mg(to).  (5.26) 


At  this  point,  we  cannot  make  any  statements  about  the  disturbance  attenuation  level 
for  this  system  because  the  right-hand  side  of  (5.26)  could  be  greater  than  zero.  We  know 
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that  the  right-hand  side  of  (5.26)  is  bounded  because  (5.21)  and  (5.22)  lead  to  bounded 
values  for  (gi(t0)  and  mg  (to).  Let  M  >  0  represent  an  upper  bound  for  the  right-hand 
side  of  (5.26).  Then, 

sup  sup  L*  <  -CJM  [t2Qo  -  3m(to)]  1  (si(h)  +  ms(t0)  <  M 
q0eJRn  w  enw  4 

=>  sup  sup  {L*  -72  (|qo|o„  +  ||w||2)}  <  M.  (5.27) 

qo eJRn  vtZ'Uw 

Inequality  (5.27)  implies  that  the  difference  between  L*  and  72  ( | qo | q0  +  Ilwl|2)  is  always 
bounded  by  a  fixed  value.  This  translates  into  a  condition  relating  the  rates  of  growth  for 
L*  and  the  disturbance  terms.  The  costs  on  the  state  plus  control  cannot  grow  at  a  faster 
rate  than  the  disturbances.  This  result  is  a  modified  form  of  disturbance  attenuation  for 
the  nonlinear  system.  We  note  that  it  is  possible  that  the  right-hand  side  of  (5.26) 
could  have  an  upper  bound  of  zero,  in  which  case  we  achieve  disturbance  attenuation 
level  7,  using  the  standard  meaning  of  the  phrase.  (Recall  that  a(0)  =  0,  which  implies 
(3 (to)  =  0.  If  /3(t)  =  0,  then  the  affine  term  vanishes,  Equation  (5.15)  becomes  linear,  and 
the  maximization  preceding  (5.26)  becomes  negative  by  construction,  as  shown  in  [86].) 
Since,  in  general,  we  have  to  account  for  the  affine  terms  which  are  sign  indefinite,  we 
can  only  guarantee  the  modified  form  of  disturbance  attenuation. 

The  portion  of  the  proof  that  shows  that  we  cannot  find  a  controller  to  achieve  a 
performance  level  of  7  for  7  <  7*(0)  is  similar  to  that  presented  in  [86]  and  is  not 
repeated  here. 

Our  analysis  above  has  thus  designed  a  control  law  for  the  imperfect  state  measure¬ 
ment  case  and  proven  a  form  of  local  disturbance  attenuation  for  the  closed-loop  system. 
We  will  simulate  the  system  with  this  controller  in  Chapter  6  to  verify  its  effectiveness 
as  a  tracking  controller. 
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5.4  Analysis  of  Controller  Design 


We  now  provide  in  this  section  a  more  detailed  analysis  of  the  imperfect  state  mea¬ 
surement  controller  in  terms  of  its  disturbance  attenuation  properties.  In  addition,  this 
section  introduces  the  problem  of  proving  local  exponential  stability  for  the  closed-loop 
system  using  a  separation  principle  argument.  Although  we  are  able  to  elaborate  on  the 
ability  of  the  system  to  attenuate  disturbances,  we  will  leave  the  establishment  of  local 
stability  results  as  an  open  research  problem. 

The  imperfect  state  measurement  tracking  controller  depends  on  the  solution  to  two 
generalized  Riccati  differential  equations  [one  for  Z(t)  and  one  for  £(£)]  and  one  ordinary 
differential  equation  for  the  estimate  of  the  state  q(f).  We  selected  this  approach  because 
of  the  disturbance  attenuation  characteristics  and  the  inherent  robustness  properties 
that  arise  from  H°°  techniques.  In  addition,  the  approach  provides  a  direct  method 
for  computing  the  control  law.  The  resulting  controller  is  based  on  a  linearization  of 
the  original  system,  so  we  can  prove  only  local  disturbance  attenuation  results  for  the 
closed- loop  system. 

One  way  to  provide  a  more  complete  description  of  the  disturbance  attenuation  prop¬ 
erties  of  the  system  is  to  solve  the  Hamilton- Jacobi  inequalities  associated  with  the  full 
nonlinear  time- varying  problem.  A  general  analytical  solution  for  these  inequalities  is 
not  readily  available  for  the  types  of  systems  we  are  considering,  and  in  fact  may  be 
impossible  to  obtain.  Numerical  solutions  may  certainly  be  possible,  but  we  decided  not 
to  pursue  this  computationally  expensive  option. 

Another  way  to  characterize  the  disturbance  attenuation  results  is  to  rewrite  (5.8)  as 

q  =  A(q)q  +  o:(q)  +  Bu  +  Z)w  (5.28) 
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where 


Dw  =  Dw  +  [i4(q)q  -  A(q)q  +  a(q)  -  a(q)] . 

This  representation  parallels  the  one  presented  in  Section  4.4  and  allows  us  to  show 
that  the  design  leads  to  an  optimal  disturbance  attenuating  controller  with  respect  to 
disturbance  w.  As  mentioned  before,  this  approach  leads  to  a  more  conservative  controller 
because  the  new  disturbance  w  is  accounting  for  the  remaining  nonlinearities  in  the 
system.  However,  this  characterization  offers  a  simple  alternative  to  the  more  complete 
description  of  the  disturbance  attenuation  properties  presented  in  Section  5.2. 

A  second  reasonable  goal  for  the  imperfect  state  measurement  controller  design,  which 
we  will  not  pursue  here,  is  to  prove  local  exponential  tracking  for  the  nonlinear  system 
using  a  separation  principle.  The  separation  principle  allows  us  to  individually  design  a 
state  feedback  controller  and  a  state  estimator,  and  then  combine  the  results  to  form  an 
output  feedback  control  law.  The  separation  principle  holds  in  general  for  linear  systems, 
but  it  must  be  demonstrated  for  each  nonlinear  system.  We  have  shown  that  the  perfect 
state  feedback  tracking  controller  is  locally  exponentially  stable,  which  is  the  first  step 
in  proving  the  results  for  the  imperfect  state  measurement  case.  We  have  also  shown 
that  the  closed-loop  imperfect  state  measurement  controller  achieves  local  disturbance 
attenuation,  which  may  be  helpful  in  establishing  the  separation  principle. 

To  complete  the  proof  of  local  exponential  tracking,  we  suggest  advancing  the  recent 
results  on  cascaded  systems  by  Loria,  Fossen,  and  Panteley  [98]  to  develop  a  separation 
principle  for  nonlinear  underactuated  systems.  There  are  two  key  challenges  with  the 
approach  in  [98]  that  must  be  resolved  in  order  to  apply  the  results  to  our  problem. 
First,  the  authors  designed  a  controller  for  a  fully  actuated  system,  so  the  results  will 
have  to  be  modified  to  handle  the  underactuated  case.  The  second  challenge  is  that  the 
control  law  in  [98]  is  a  proportional-integral-derivative  controller,  which  greatly  simplifies 
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the  analysis  of  the  closed-loop  system.  The  system  using  the  H°°  controller  is  more 
mathematically  involved,  because  it  relies  on  coupled  differential  equations  to  compute 
the  control  law.  Even  though  the  computations  are  more  involved  for  the  H°°  case,  the 
approach  is  well  understood  and  there  are  techniques  to  analyze  the  controller,  which 
should  lead  to  a  separation  principle  for  the  nonlinear  underactuated  system.  Having 
formulated  this  problem  and  outlined  one  solution  approach,  we  plan  to  leave  this  issue 
as  an  open  research  area  for  future  work. 


5.5  Imperfect  State  Measurement  Summary 

This  chapter  constructed  an  imperfect  state  measurement  tracking  controller  by  com¬ 
bining  an  if°°-optimal  state  estimate  with  the  perfect  state  feedback  controller  developed 
in  Chapter  4.  Using  recent  results  on  the  robustness  of  minimax  controllers  to  nonlinear 
perturbations  [86],  we  were  able  to  prove  a  form  of  disturbance  attenuation  for  the  closed- 
loop  system.  The  imperfect  state  measurement  tracking  controller  is  the  final  element  in 
the  design  for  planning  and  controlling  the  motion  of  an  underactuated  vehicle.  We  are 
now  in  a  position  to  combine  all  of  the  elements  and  demonstrate  how  well  they  perform 
through  a  series  of  simulations. 
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CHAPTER  6 


SIMULATION  RESULTS 


This  chapter  presents  the  results  of  simulations  that  implement  the  motion  plan¬ 
ning,  the  perfect  state  feedback  control,  and  the  imperfect  state  measurement  control 
algorithms  described  in  Chapters  3,  4,  and  5.  We  will  explain  the  details  behind  each 
simulation  and  comment  on  the  results. 

Except  where  noted,  for  most  of  the  simulations  we  used  the  following  values  for  the 
coefficients  in  the  underactuated  vehicle  model  given  by  (1.1)  through  (1.3): 

mu  =  0.5,  mv  —  —2.0,  mr  =  0.5 
du  =  1.0,  dv  =  2.0,  dr  =  1.0. 

These  parameter  values  represent  a  scaled  realistic  model.  When  we  report  a  configura¬ 
tion  for  the  system,  the  order  of  the  variables  will  be  [ u ,  v,  r,  x,  y,  ip]. 

6.1  Motion  Planning 

Motion  planning  is  the  first  step  in  controlling  the  underactuated  vehicle.  The  tech¬ 
niques  presented  in  Chapter  3  allow  us  to  move  the  vehicle  between  a  variety  of  con¬ 
figurations  and  generate  very  reasonable  trajectories.  The  simulations  presented  in  this 
section  illustrate  the  results  of  the  motion  planning  algorithm  with  a  sample  of  the  types 
of  motions  that  can  be  generated. 
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6.1.1  Basic  motion  planning  examples 

We  applied  the  basic  motion  planning  algorithm  to  five  example  problems  to  demon¬ 
strate  its  features.  The  first  example  shown  in  Figure  6.1  illustrates  how  the  iterative 
//“-filter  helps  improve  the  feasible  trajectory  and  make  the  vehicle’s  final  configuration 
approach  the  goal  configuration.  In  this  example,  we  left  the  initial  configuration  of 
the  vehicle  as  its  actual  configuration  for  the  motion  planning  algorithm.  This  design 
choice  appears  in  Figure  6.1  as  a  difference  between  the  candidate  position  curve  (dotted 
line)  and  the  feasible  planned  position  curves  along  the  initial  segment  of  the  path.  This 
difference  does  not  diminish  as  the  iterative  //“-filter  operates  on  the  system. 

y 


Figure  6.1  Basic  motion  planning  results  using  the  iterative  //“-filter.  The  dotted  line 
shows  the  candidate  polynomial  position  curve,  the  solid  line  shows  the  first  feasible 
trajectory,  and  the  dashed  lines  show  the  feasible  trajectories  as  the  //“-filter  iterates 
on  the  results. 

Initial  Configuration:  [1,  0,  0,  0,  0,  0] 

Goal  Configuration:  [1,  0,  0,  8,  8,  0] 

Final  Configuration:  [1.00,  0.10,  -0.09,  7.89,  7.81,  -0.06] 
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As  described  in  Chapter  3,  we  can  reduce  the  initial  error  between  the  candidate 
position  curve  and  the  feasible  position  curve  by  using  the  estimated  initial  conditions  to 
plan  the  vehicle’s  motion.  Figure  6.2  shows  the  results  of  applying  this  technique.  In  this 
case,  we  did  not  have  to  apply  the  iterative  H°°- filter  to  achieve  a  small  final  configuration 
error.  Figure  6.3  illustrates  the  second  method  for  correcting  the  initial  configuration  to 
improve  the  results.  In  this  example,  we  developed  a  motion  using  the  iterative  H 00 -filter 
and  the  actual  initial  configuration  and  then  shifted  the  initial  orientation  and  position 
to  completely  eliminate  orientation  and  position  errors  at  the  final  configuration.  The 
amount  by  which  we  shift  the  initial  configuration  is  relatively  small  and,  if  it  falls  within 
the  region  of  attraction  for  the  tracking  controllers,  we  will  be  able  to  follow  the  new 
feasible  trajectory.  Table  6.1  summarizes  the  initial  and  final  configurations  for  the  first 
three  motion  planning  examples, 
y 


Figure  6.2  Basic  motion  planning  results  using  the  estimated  initial  conditions.  This 
example  did  not  use  the  iterative  H°° -filter.  The  final  planned  configuration  is  very  close 
to  the  goal  configuration. 

Initial  Configuration:  [0.99,  -0.12,  0.15,  0,  0,  0.12] 

Goal  Configuration:  [1,  0,  0,  8,  8,  0] 

Final  Configuration:  [0.99,  0.10,  -0.10,  7.92,  7.94,  -0.12] 
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Figure  6.3  Basic  motion  planning  results  with  the  corrected  initial  configuration. 
Part  (a)  shows  the  original  position  curve  and  part  (b)  shows  the  curve  after  the  correc¬ 
tion.  Note  that  the  values  for  the  final  planned  position  and  orientation  exactly  match 
the  corresponding  values  in  the  goal  configuration. 

Initial  Configuration:  [1,  0,  0,  0.55,  -0.23,  0.06] 

Goal  Configuration:  [1,  0,  0,  8,  8,  0] 

Final  Configuration:  [1.00,  0.10,  -0.09,  8,  8,  0] 


Table  6.1  Basic  motion  planning  results  with  different  initial  configurations.  The  table 
compares  the  initial  and  final  configurations  for  three  different  variations  of  the  basic 
motion  planning  algorithm.  The  three  variations  are  to  use  the  iterative  //“-filter,  to 
use  the  estimated  initial  configuration,  and  to  correct  the  initial  configuration  to  exactly 
match  the  final  position  and  orientation  requirements. 


Algorithm 

Initial  Configuration 

Final  Configuration 

//“-Filter 

[1,  0,  0,  0,  0,  0] 

[1.00,  0.10,  -0.09,  7.89,  7.81,  -0.06] 

Estimated  Initial 

[0.99,  -0.12,  0.15,  0,  0,  0.12] 

[0.99,  0.10,  -0.10,  7.92,  7.94,  -0.12] 

Corrected  Initial 

[1,  0,  0,  0.55,  -0.23,  0.06] 

[1,  0.10,  -0.09,  8,  8,  0] 
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Figures  6.4  and  6.5  provide  two  more  examples  of  feasible  motions  developed  using 
the  basic  motion  planning  algorithms.  Figure  6.4  shows  a  U-turn  motion  and  Figure  6.5 
displays  how  the  vehicle  would  make  a  lateral  displacement.  These  examples  verify  that 
the  planning  approach  can  solve  a  wide  range  of  practical  problems  for  an  underactuated 
vehicle. 

y 


Figure  6.4  Additional  basic  motion  planning  results  using  the  estimated  initial  configu¬ 
ration.  The  motion  is  a  feasible  position  curve  for  a  U-turn  and  it  relied  on  the  if°°-filter 
to  improve  the  results. 

Initial  Configuration:  [0.99,  -0.15,  0.16,  0,  0,  0.15] 

Goal  Configuration:  [1,  0,  0,  0,  10,  7r] 

Final  Configuration:  [0.99,  -0.14,  0.14,  0.02,  9.94,  3.30] 

6.1.2  Extensions  to  basic  motion  planning  examples 

To  solve  the  obstacle  avoidance  and  multiple  vehicle  planning  problems,  we  combined 
aspects  of  the  basic  motion  planning  algorithm  with  the  rapidly-exploring  random  trees 
search  technique  to  develop  feasible  plans.  Figures  6.6  and  6.7  illustrate  two  examples 
of  these  techniques.  In  Figure  6.6,  we  planned  the  motion  for  a  single  vehicle  around 
a  set  of  obstacles.  The  figure  shows  the  raw  position  curve  as  planned  by  the  modified 
RRT  software  as  well  as  the  smoothed  curve  after  applying  the  iterative  i7°°-filter.  This 
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Figure  6.5  Additional  basic  motion  planning  results  using  the  estimated  initial  config¬ 
uration.  The  motion  is  a  feasible  position  curve  for  a  lateral  displacement  and  it  did  not 
require  the  H°°- filter  to  improve  the  results. 

Initial  Configuration:  [0.99,  -0.15,  0.22,  0,  0,  0.15] 

Goal  Configuration:  [1,  0,  0,  0,  10,  0] 

Final  Configuration:  [0.99,  0.10,  -0.10,  -0.31,  9.69,  -0.12] 

example  highlights  how  the  H°° -filter  softens  the  transition  between  the  two  rapidly- 
exploring  trees  that  appears  as  a  zigzag  at  coordinates  (x,  y )  =  (55,  47)  in  Figure  6.6(a), 
but  is  absent  in  Figure  6.6(b). 

Figure  6.7  illustrates  the  results  of  planning  motions  for  two  vehicles  around  obstacles 
and  applying  the  sampling  smoothing  approach  to  improve  the  trajectories.  To  develop 
the  smoothed  trajectories  in  Figure  6.7(b),  we  sampled  the  trajectories  for  each  vehicle  at 
the  initial  and  final  configurations  and  at  two  intermediate  configurations.  We  then  used 
the  basic  motion  planning  algorithm  to  plan  motions  between  successive  configurations 
to  achieve  substantially  smoother  feasible  trajectories.  In  this  case,  we  required  a  min¬ 
imum  amount  of  sampling  to  capture  the  important  aspects  of  the  vehicles’  motions  to 
avoid  collisions.  Other  examples  may  require  more  intermediate  sample  points  to  achieve 
similar  results. 
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Figure  6.6  Motion  planning  with  obstacles  in  the  environment.  Part  (a)  shows  the 
results  of  the  RRT  algorithm  and  part  (b)  shows  how  smoothing  the  trajectory  with  the 
iterative  ff°°-filter  improves  the  results.  Both  trajectories  are  feasible.  The  dark  boxes 
are  the  obstacles. 

Initial  Configuration:  [1,  0,  0,  40,  40,  0] 

Goal  Configuration:  [1,  0,  0,  50,  50,  0] 

Final  Configuration:  [1.21,  -0.03,  0.01,  48.61,  49.36,  0.11] 
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(b) 


Figure  6.7  Motion  planning  for  multiple  vehicles  with  obstacles.  Part  (a)  shows  the 
results  of  the  RRT  algorithm  and  part  (b)  shows  how  smoothing  the  trajectories  with 
sampling  improves  the  results.  We  used  the  basic  motion  planning  algorithm  to  plan  the 
motions  between  the  sample  points.  Both  trajectories  are  feasible. 

Vehicle  1  Vehicle  2 

Initial  Configurations:  [1,  0,  0,  40,  40,  0]  [1,  0,  0,  40,  50,  0] 

Goal  Configurations:  [1,  0,  0,  50,  50,  0]  [1,  0,  0,  50,  40,  0] 

Final  Configurations:  [0.96,  0,  0,  49.74,  50,  0]  [0.99,  -0.11,  0.11,  49.62,  38.61,  0.06] 
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6.2  Perfect  State  Feedback  Examples 


The  perfect  state  feedback  tracking  controller  forces  the  vehicle  to  follow  the  feasible 
trajectories  generated  by  the  motion  planning  algorithms.  Figures  6.8  and  6.9  illustrate 
the  results  of  applying  this  controller  when  different  disturbances  are  present  and  the 
vehicle  has  significantly  different  initial  conditions.  In  Figure  6.8,  the  system  is  subjected 
to  a  white  noise  disturbance  with  a  =  0.5  and  the  vehicle’s  initial  configuration  aligns 
with  the  expected  initial  configuration.  The  controller  successfully  rejects  the  influence 
of  the  noise  to  follow  the  feasible  trajectory  and  approach  the  desired  final  configuration. 

Figure  6.9  illustrates  the  effect  of  applying  a  sinusoidal  disturbance  to  the  system  and 
giving  the  vehicle  an  exceptionally  large  initial  configuration  error.  This  type  of  large 
initial  error  could  occur  if  the  underactuated  controller  was  used  as  a  backup  system 
for  a  fully  actuated  vehicle.  In  this  case,  there  would  be  a  delay  between  the  actuator 
failure  and  the  application  of  the  new  controller,  which  would  allow  the  vehicle  to  drift 
away  from  the  intended  trajectory.  Section  6.6  provides  a  more  detailed  discussion  of  this 
example.  Even  though  the  initial  configuration  is  drastically  different  from  the  expected 
initial  configuration,  the  tracking  controller  corrects  the  error  using  a  natural  motion 
and  makes  the  vehicle  approach  the  feasible  trajectory.  The  sinusoidal  nature  of  the 
disturbances  is  visible  in  the  velocity  plots  in  Figure  6.9(a)  and  in  the  control  input  plots 
in  Figure  6.9(c). 

To  verify  that  the  tracking  control  algorithm  works  for  the  obstacle  avoidance  and 
multiple  vehicle  cases,  we  applied  the  perfect  state  feedback  tracking  controller  to  the 
motions  planned  in  Figures  6.6  and  6.7.  Figure  6.10  displays  the  results.  In  both  cases, 
the  vehicles  tracked  the  feasible  trajectories  without  collisions,  as  expected. 
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Figure  6.8  Perfect  state  feedback  trajectory  tracking.  Part  (a)  shows  the  desired  (dashed 
lines)  and  actual  (solid  lines)  velocities  for  the  vehicle,  part  (b)  shows  the  positions  and 
orientations,  part  (c)  shows  the  two  control  inputs,  and  part  (d)  displays  stop-motion 
images  of  the  vehicle  as  it  tracks  the  x-y  position  curve.  The  disturbances  are  white  noise 
with  a  —  0.5. 

Initial  Configuration:  [1,  0,  0,  0,  0,  0] 

Goal  Configuration:  [1,  0,  0,  8,  8,  0] 

Final  Configuration:  [1.06,  -0.08,  0.03,  7.84,  7.98,  0.01] 
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(c)  (d) 


Figure  6.9  Perfect  state  feedback  trajectory  tracking  with  poor  initial  conditions.  The 
vehicle  has  a  significant  displacement  from  the  desired  trajectory,  but  the  tracking  con¬ 
troller  is  able  to  generate  a  natural  motion  to  recover  from  the  initial  error.  The  distur¬ 
bances  are  sinusoidal  signals  with  an  amplitude  of  0.1  and  a  frequency  of  0.2  Hz. 

Initial  Configuration:  [1,  0,  0,  6,  2,  0] 

Goal  Configuration:  [1,  0,  0,  8,  8,  0] 

Final  Configuration:  [1 .04,  0.11,  -0.05,  7.91,  7.86,  -0.14] 
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Figure  6.10  Perfect  state  feedback  tracking  with  obstacles  and  multiple  vehicles. 
Part  (a)  shows  stop-motion  images  of  the  vehicle  tracking  the  smoothed  x-y  position 
curve  around  obstacles,  as  planned  in  Figure  6.6.  Part  (b)  shows  stop-motion  images  for 
two  vehicles  with  coordinated  motions  around  obstacles,  as  planned  in  Figure  6.7. 
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6.3  Imperfect  State  Measurement  Examples 

The  imperfect  state  measurement  controller  can  only  directly  access  the  position  and 
orientation  variables  for  feedback,  so  it  must  develop  estimates  for  the  other  three  states 
to  implement  the  control  law.  Figures  6.11  and  6.12  depict  the  results  of  applying  the 
imperfect  state  measurement  controller  to  the  same  examples  illustrated  in  Figures  6.8 
and  6.9.  The  figures  corresponding  to  the  imperfect  state  measurement  case  include 
estimates  for  the  state  variables  as  dotted  lines  in  parts  (a)  and  (b)  of  the  figures. 

Comparing  Figure  6.11  with  Figure  6.8,  we  observe  that  the  tracking  performance  for 
the  imperfect  state  measurement  case  is  not  as  good  as  for  the  perfect  state  measurement 
case.  We  expect  the  imperfect  state  measurement  approach  will  produce  larger  errors 
because  of  the  extra  estimation  required  in  the  algorithm,  but  the  final  configuration 
errors  are  similar  for  the  two  approaches  and  the  overall  results  are  acceptable. 

In  Figure  6.12  with  the  imperfect  state  measurement  controller,  we  note  that  the 
vehicle  actually  converges  on  the  desired  position  curve  sooner  than  in  Figure  6.9,  where 
we  applied  the  perfect  state  feedback  control  law.  This  occurs  because  the  imperfect  state 
measurement  controller  is  using  the  state  estimates  to  develop  the  control  inputs  and,  in 
this  case,  the  estimates  positively  interact  with  the  initial  configuration  to  improve  the 
overall  performance.  We  expect  this  performance  to  be  atypical  for  the  imperfect  state 
measurement  approach. 

To  complement  the  examples  for  the  imperfect  state  measurement  controller,  we  now 
present  a  small  set  of  simulations  that  demonstrate  the  validity  of  Assumption  A5.1  for 
an  underactuated  ship  model.  The  three  figures  show  how  changes  in  the  disturbance 
term  w  affect  the  eigenvalues  of  F(t )  and  the  values  in  (5{t).  Section  5.3  defines  these 
terms  and  explains  their  importance  to  Assumption  A5.1.  Each  simulation  is  based  on 
the  vehicle  tracking  a  feasible  circular  trajectory.  Figures  6.13  through  6.15  each  contain 
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Figure  6.11  Imperfect  state  measurement  tracking  control.  Part  (a)  shows  the  desired 
(dashed  lines),  estimated  (dotted  lines),  and  actual  (solid  lines)  velocities  for  the  vehicle; 
part  (b)  shows  the  positions  and  orientations;  part  (c)  shows  the  two  control  inputs;  and 
part  (d)  displays  stop-motion  images  of  the  vehicle  as  it  tracks  the  x-y  position  curve. 

The  disturbances  are  white  noise  with  a  =  0.5. 

Initial  Configuration:  [1,  0,  0,  0,  0,  0] 

Goal  Configuration:  [1,  0,  0,  8,  8,  0] 

Final  Configuration:  [1.05,  0.15,  -0.07,  7.79,  8.29,  0.03] 
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Figure  6.12  Imperfect  state  measurement  case  with  poor  initial  conditions.  The  system 
disturbances  are  sinusoidal  signals  with  an  amplitude  of  0.1  and  a  frequency  of  0.2  Hz, 
and  the  measurement  disturbances  are  white  noise  with  a  —  0.5. 

Initial  Configuration:  [1,  0,  0,  0,  0,  0] 

Goal  Configuration:  [1,  0,  0,  8,  8,  0] 

Final  Configuration:  [1 .04,  0.12,  -0.05,  7.91,  7.90,  -0.14] 
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three  parts.  Parts  (a)  and  (b)  plot  the  eigenvalues  of  F(t)  versus  time,  and  part  (c) 
displays  the  nonzero  elements  of  /3(t)  versus  time.  In  each  plot,  the  solid  line  shows 
the  results  for  some  nonzero  disturbance  and,  for  comparison,  the  dashed  line  shows  the 
values  when  there  are  no  disturbances. 

Figure  6.13  applies  the  worst-case  disturbance  and  the  other  figures  use  sampled  white 
noise  with  a  set  to  0.5  and  5.  In  each  case,  the  disturbances  have  a  relatively  mild  effect 
on  the  eigenvalues  of  F(t)  and  on  /3(t ).  With  a  =  5  in  Figure  6.15,  the  role  of  the  noise 
is  more  prominent,  but  the  magnitude  of  the  changes  is  not  significantly  different  from 
that  with  a  =  0.5  in  Figure  6.14. 

These  simulations  show  that  Assumption  A5.1  is  reasonable  for  the  desired  trajectory 
examined  is  this  case.  The  validity  of  Assumption  A5.1  should  be  verified  for  each  system 
and  for  all  desired  trajectories  to  ensure  the  controller  designed  in  Section  5.3  will  deliver 
acceptable  performance. 

6.4  Comparison  of  Results  to  Existing  Techniques 

To  help  judge  the  value  of  the  controller  designs,  we  compared  the  tracking  perfor¬ 
mance  to  two  other  controller  design  techniques  for  underactuated  vehicles.  The  other 
techniques  were  developed  by  Godhavn  [51]  and  by  Pettersen  and  Nijmeijer  [3].  As  de¬ 
scribed  in  Chapter  2,  both  of  these  approaches  rely  on  backstepping  to  develop  their  re¬ 
sults.  We  provide  a  single  simulation  example  that  highlights  the  key  differences  between 
our  approach  and  the  other  two  controller  designs.  Figure  6.16  shows  the  stop-motion 
images  of  the  vehicle  following  the  x-y  curve  using  the  state  feedback  tracking  controller 
designed  in  Chapter  4.  The  motion  planning  algorithm  assumed  that  the  vehicle  had 
an  initial  orientation  of  t/>(£0)  =  0,  but  the  actual  orientation  was  ip(t0)  =  7 r.  The  state 
feedback  controller  was  able  to  correct  for  the  initial  error  and  make  the  vehicle  converge 
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Figure  6.13  Analysis  of  terms  influenced  by  the  disturbance.  Parts  (a)  and  (b)  show 
the  12  eigenvalues  of  F(t)  plotted  versus  time.  Part  (c)  shows  the  values  of  the  three 
nonzero  elements  of  j3(t)  versus  time.  The  solid  lines  are  the  values  when  the  disturbance 
is  the  worst-case  disturbance,  and  the  dashed  lines  show  the  values  when  there  are  no 
disturbances. 
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Figure  6.14  Analysis  of  terms  influenced  by  the  disturbance.  Parts  (a)  and  (b)  show 
the  12  eigenvalues  of  F(t)  plotted  versus  time.  Part  (c)  shows  the  values  of  the  three 
nonzero  elements  of  f3(t)  versus  time.  The  solid  lines  are  the  values  when  the  disturbance 
is  the  sampled  white  noise  with  a  —  0.5,  and  the  dashed  lines  show  the  values  when  there 
are  no  disturbances. 
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Figure  6.15  Analysis  of  terms  influenced  by  the  disturbance.  Parts  (a)  and  (b)  show 
the  12  eigenvalues  of  F(t)  plotted  versus  time.  Part  (c)  shows  the  values  of  the  three 
nonzero  elements  of  fJ(t)  versus  time.  The  solid  lines  are  the  values  when  the  disturbance 
is  the  sampled  white  noise  with  a  —  5.0,  and  the  dashed  lines  show  the  values  when  there 
are  no  disturbances. 


115 


on  the  desired  trajectory.  There  is  a  small  final  orientation  error,  but  the  final  position 
and  velocities  are  close  to  the  desired  values. 

y 


Figure  6.16  Comparison  of  perfect  state  feedback  tracking  to  other  techniques.  The 
motion  planning  algorithm  assumed  the  vehicle  had  an  initial  orientation  of  ip(to)  =  0, 
but  the  actual  orientation  was  ip  (to)  =  7 r.  The  state  feedback  controller  corrects  for 
the  initial  configuration  error  and  forces  the  vehicle  to  track  the  desired  trajectory, 
even  though  there  are  white  noise  disturbances  with  a  =  0.5  affecting  the  system. 
Actual  Initial  Configuration:  [1,  0,  0,  0,  0,  7r] 

Expected  Initial  Configuration:  [1,  0,  0,  0,  0,  0] 

Goal  Configuration:  [1,  0,  0,  0,  10,  it] 

Final  Configuration:  [1.04,  -0.32,  0.26,  0.09,  9.88,  3.46] 


The  results  shown  in  Figure  6.16  are  an  improvement  over  the  technique  proposed 
by  Godhavn  for  two  reasons.  First,  the  tracking  controller  developed  in  Chapter  4  cor¬ 
rects  the  orientation  of  the  vehicle.  Godhavn’s  backstepping  controller  does  not  penalize 
configurations  when  the  vehicle  is  oriented  backwards  along  the  trajectory  and  cannot 
correct  this  situation.  Godhavn’s  technique  would  force  the  vehicle  to  move  backwards 
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along  the  path.  The  second  advantage  of  our  approach  is  that  the  technique  is  designed 
to  be  robust  to  disturbances  and  to  attenuate  their  effect  on  the  system.  As  shown 
in  Figure  6.16,  the  vehicle  still  captures  the  desired  trajectory  when  disturbances  are 
present.  The  backstepping  derivation  requires  an  accurate  model  of  the  system  and  does 
not  consider  the  effect  of  disturbances.  Godhavn  remarks  in  [51]  that  disturbances  can  be 
included  in  the  model  and  addressed  with  backstepping,  but  does  not  present  the  results. 
We  should  note  that  Godhavn’s  approach  is  still  a  reasonable  method  for  performing 
tracking  for  an  underactuated  vehicle  if  the  initial  configuration  has  a  small  orientation 
error  and  there  are  no  disturbances  in  the  system. 

Our  techniques  also  offer  some  improvements  over  those  developed  by  Pettersen  and 
Nijmeijer  [3].  The  results  in  Figure  6.16  show  that  the  vehicle  converges  to  the  desired  x- 
y  position  curve  after  correcting  the  initial  error.  The  simulations  reported  in  [3]  indicate 
that  the  controller  was  not  able  to  make  the  vehicle  converge  to  the  desired  position  curve. 
As  in  our  example,  the  desired  position  curve  in  [3]  was  a  circular  arc,  but  the  vehicle 
always  maintained  a  slight  offset  from  the  desired  position.  In  addition,  one  version  of 
the  Pettersen  and  Nijmeijer  controller  was  designed  with  pure  backstepping  and  did  not 
perform  well  with  unmodeled  dynamics.  The  authors  do  describe  another  controller  to 
correct  for  this  deficiency,  which  makes  their  approach  robust  to  modeling  errors.  With 
the  exception  of  the  minor  convergence  problem,  the  Pettersen  and  Nijmeijer  approach 
offers  a  viable  alternative  to  our  design, 


6.5  Extension  of  Techniques  to  Additional  Models 

In  this  section  we  will  consider  three  extensions  to  the  basic  model  used  for  the 
majority  of  the  experiments.  First,  we  will  consider  the  performance  of  the  motion 
planning  and  control  algorithms  when  we  introduce  a  nonminimum  phase  behavior  into 
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the  model.  Next  we  will  change  the  parameter  values  in  the  model  to  demonstrate  that 
the  techniques  work  for  vehicles  with  different  properties.  Finally,  we  will  provide  a  few 
simulations  that  illustrate  how  the  state  feedback  tracking  controllers  perform  when  the 
disturbances  affect  the  unactuated  directions  in  the  model. 

We  performed  a  set  of  three  simulations  for  the  nonminimum  phase  system  described 
in  Section  4.5  and  compared  the  results  to  the  original  system  with  kv  =  0.  To  simplify  the 
comparison,  we  set  the  disturbances  to  zero,  but  including  disturbances  should  not  signif¬ 
icantly  alter  the  results.  In  each  simulation,  the  system  started  at  q  =  [1,  0,  0,  0,  0,  0]T. 
Table  6.2  summarizes  the  results  of  the  three  experiments.  In  each  case,  the  final  tracking 
performance  was  similar  for  the  original  system  and  the  nonminimum  phase  formulation. 
There  were  only  minor  and  insignificant  differences  between  the  results.  These  experi¬ 
ments  indicate  that  the  motion  planning  and  control  algorithms  perform  well  for  both 
minimum  and  nonminimum  phase  systems. 

The  second  extension  to  the  original  vehicle  model  shows  that  the  motion  planning 
and  tracking  algorithms  still  provide  good  solutions  when  we  fundamentally  change  the 
parameters  in  the  model.  We  selected  the  following  values  for  the  model  parameters: 

mu  =  2.0,  mv  =  —0.5,  mr  =  —2.5 
du  =  2.0,  dv  =  1.0,  dr  —  5.0. 

Table  6.2  Comparison  of  minimum  and  nonminimum  phase  systems.  The  final  config¬ 
urations  are  nearly  identical  for  the  two  approaches. 

Desired  Final  Minimum  Phase  Final  Nonminimum  Phase  Final 

[1,  0,  0,  8,  8,  0]  [0.99,  0.10,  -0.10,  7.92,  7.94,  -0.11]  [1.00,  0.09,  -0.10,  7.85,  7.95,  -0.12] 

[1,  0,  0,  0,  10,  0]  [0.99,  0.10,  -0.10,  -0.31,  9.69,  -0.12]  [0.98,  0.08,  -0.07,  -0.19,  9.59,  -0.20] 

[1,  0,  0,  0,  10,  t r]  [0.99,  -0.14,  0.14,  0.02,  9.94,  3.30]  [0.99,  -0.13,  0.13,  -0.07,  10.01,  3.28] 
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These  new  parameter  values  contain  two  significant  changes  from  the  original  set.  First, 
we  exchanged  the  relative  magnitudes  of  mu  and  mv  and,  second,  we  changed  the  sign  of 
mr.  These  values  represent  a  change  in  the  shape  of  the  vehicle  from  one  that  moves  with 
its  major  axis  perpendicular  to  the  forward  velocity  to  one  with  the  major  axis  aligned 
with  the  forward  velocity.  We  simulated  this  system  with  a  motion  from  [1,  0,  0,  0,  0,  0] 
to  [1,  0,  0,  8,  8,  0]  and  Figure  6.17  displays  the  results.  The  motion  planning  and  perfect 
state  feedback  tracking  control  algorithms  successfully  handled  a  fundamental  change  in 
the  parameter  values  for  the  model. 
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Figure  6.17  Motion  planning  and  tracking  with  a  new  vehicle  model.  The  algorithms 
successfully  planned  the  motion  and  controlled  the  vehicle  after  a  fundamental  change  in 
the  model  parameter  values. 

The  final  set  of  simulations  in  this  section  examines  how  well  the  tracking  controller 
attenuates  different  types  of  disturbances.  We  used  the  basic  motion  planning  algorithm 
to  plan  a  motion  from  [1,  0,  0,  0,  0,  0]  to  [1,  0,  0,  8,  8,  0],  and  then  subjected  the  system 
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to  four  different  types  of  disturbances.  The  first  simulation  applied  disturbances  with 
constant  amplitude  signals  along  the  actuated  directions  and  no  signal  in  the  unactuated 
direction,  so  that  we  had  w  =  [wi,W2,Wz]t  —  [0.1,0,0.1]T,  in  the  equations  of  motion. 
For  the  second  simulation,  we  applied  the  constant  amplitude  disturbance  in  only  the 
unactuated  direction,  so  that  we  had  w  =  [0, 0.1, 0]T.  The  third  and  fourth  simulations 
mirrored  the  first  two  in  terms  of  the  directions  the  disturbances  influenced,  but  the 
constant  amplitude  signals  were  replaced  with  sinusoidal  signals  of  the  form  Wi(t)  = 
0.1  cos[27r(0.2)f].  Table  6.3  summarizes  the  results  of  applying  the  perfect  state  feedback 
tracking  controller  with  the  different  types  of  disturbances.  The  controller  successfully 
compensates  for  the  disturbances  regardless  of  their  direction.  In  all  cases,  the  controller 
was  able  to  force  the  vehicle  to  approach  the  desired  final  configuration  at  the  appointed 
time.  The  velocity  errors  for  v,  the  unactuated  direction,  were  slightly  larger  in  the  two 
cases  where  the  disturbances  affected  only  that  direction,  but  the  final  configurations 
were  still  very  close  to  the  desired  values. 

We  performed  additional  simulations  where  the  disturbances  depended  on  the  ori¬ 
entation  of  the  vehicle.  This  situation  would  arise  if  the  vehicle  was  turning  and  the 
disturbances  were  fixed  with  respect  to  the  inertial  frame.  An  example  of  this  case  would 
be  a  ship  turning  while  the  waves  continued  to  move  in  a  constant  direction.  The  perfect 
state  feedback  tracking  controller  successfully  handled  this  situation  by  rejecting  the  dis- 


Table  6.3  Disturbance  attenuation  results  with  disturbances  affecting  different  direc¬ 
tions.  The  table  shows  the  final  configurations  using  the  perfect  state  feedback  tracking 
controller  with  the  various  combinations  of  disturbances.  The  controller  attenuates  the 
influence  of  the  disturbances  even  when  they  only  affect  the  unactuated  directions  in  the 
system. 

Directions  Constant  Amplitude  Sinusoidal 

Actuated  [1.04,  0.07,  -0.05,  8.00,  8.10,  -0.05]  [1.04,  0.07,  -0.05,  7.91,  7.90,  -0.14] 

Unactuated  [1.01,  0.13,  -0.08,  7.89,  8.11,  -0.13]  [0.99,  0.13,  -0.09,  7.93,  7.91,  -0.12] 
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turbances,  regardless  of  the  vehicle’s  orientation,  and  approximately  tracking  the  desired 
trajectory. 

Taken  together,  the  three  extensions  to  the  motion  planning  and  control  algorithms 
described  in  this  section  convincingly  show  that  the  approach  can  be  generalized  to  handle 
different  types  of  systems  and  is  robust  enough  to  neutralize  the  effect  of  a  variety  of 
disturbances. 


6.6  Failure  Recovery  for  a  Fully  Actuated  Vehicle 

One  potential  application  for  the  ability  to  plan  motions  for  an  underactuated  vehicle 
is  to  allow  a  fully  actuated  vehicle  to  gracefully  recover  from  an  actuator  failure.  As  men¬ 
tioned  in  Chapter  1,  this  capability  could  be  extremely  helpful  for  aircraft  or  spacecraft 
systems  where  an  actuator  failure  can  mean  the  difference  between  being  able  to  control 
the  vehicle  and  suffering  a  catastrophic  loss.  The  motion  planning  and  tracking  control 
problems  for  the  fully  actuated  system  are  fundamentally  different  from  those  for  an  un¬ 
deractuated  system  because,  in  the  fully  actuated  case,  the  vehicle  can  potentially  follow 
any  reference  trajectory.  The  only  limitations  on  the  motion  would  be  from  actuator 
saturation.  If  the  system  suffers  a  failure,  the  control  law  designed  for  the  fully  actuated 
case  will  not  have  the  desired  effect  and  we  will  have  to  replan  the  motion  and  apply  a 
new  controller  to  satisfy  the  motion  requirements.  We  will  outline  how  the  motion  plan¬ 
ning  and  control  algorithms  for  an  underactuated  vehicle  could  serve  as  a  key  element  in 
a  failure  recovery  system  for  a  fully  actuated  vehicle.  We  will  leave  experimentation  and 
further  study  of  this  topic  as  an  area  for  future  research. 

Suppose  we  are  given  a  model  for  a  fully  actuated  system  that  may  be  subject  to 
actuator  failure  during  normal  operation.  If  an  actuator  failed,  we  would  execute  the 
following  actions  to  maintain  control  over  the  vehicle.  First,  we  would  have  to  identify 
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the  actuator  failure  and  adjust  the  model  of  the  vehicle  to  reflect  the  change  in  dynamics. 
Identifying  the  failure  can  be  a  nontrivial  problem.  One  clear  indication  of  a  failure 
would  be  that  the  vehicle  is  no  longer  properly  tracking  planned  motions.  For  critical 
applications,  the  vehicle  could  also  be  equipped  with  sensors  to  indicate  actuator  failures, 
but  the  sensors  add  an  extra  layer  of  expense  and  complexity  which  may  not  be  desirable. 
It  is  reasonable  to  assume  that  we  will  not  be  able  to  completely  identify  the  new  system 
model  and  there  will  be  unmodeled  dynamics  in  the  revised  model.  These  unmodeled 
dynamics  can  be  considered  as  disturbances  for  the  new  control  law  to  attenuate. 

Once  the  actuator  failure  is  identified  and  the  model  corrected,  we  can  use  the  original 
motion  plan  for  the  vehicle  and  the  basic  motion  planning  algorithm  to  develop  a  set  of 
alternative  feasible  motions  for  the  vehicle.  With  a  new  set  of  feasible  motions,  we 
could  apply  one  of  the  tracking  controllers  developed  in  Chapters  4  or  5  to  control  the 
vehicle’s  motion.  Since  we  used  the  H°°  design  approach,  the  tracking  controllers  are 
robust  and  can  handle  a  variety  of  bounded  disturbances,  including  those  that  result 
from  unmodeled  dynamics.  The  revised  system  may  not  be  able  to  complete  the  original 
mission  objectives,  but  the  underactuated  design  may  provide  sufficient  control  over  the 
vehicle  to  allow  us  to  bring  it  to  a  safe  configuration  where  repairs  can  be  made. 

We  presented  a  brief  characterization  of  how  the  motion  planning  and  control  algo¬ 
rithms  for  an  underactuated  vehicle  could  be  used  to  assist  a  fully  actuated  vehicle  that 
suffered  an  actuator  failure.  These  ideas  point  to  a  line  of  research  that  is  rich  with 
opportunity  for  future  study. 


6.7  Computational  Complexity 

As  noted  above,  we  completed  most  of  the  motion  planning  and  tracking  control  sim¬ 
ulations  using  the  Mathematica  software.  This  software  allowed  us  to  solve  the  problems 
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by  writing  a  high-level  code  that  is  very  similar  to  the  equations  used  to  describe  the 
original  problems.  The  drawback  with  using  a  high-level  language  such  as  Mathematica 
is  that  the  software  involves  a  significant  amount  of  processing  overhead,  which  increases 
the  time  required  to  complete  the  simulations.  To  develop  more  efficient  software,  the 
motion  planning  and  tracking  algorithms  could  be  implemented  in  C/C++,  which  would 
allow  us  to  eliminate  the  processing  overhead  associated  with  Mathematica  and  decrease 
the  simulation  times.  We  will  leave  these  types  of  enhancements  to  the  simulations  as 
an  area  for  future  study. 

Regardless  of  the  manner  in  which  we  implement  the  motion  planning  and  tracking 
control  algorithms,  the  overall  computational  complexity  of  the  algorithms  will  remain 
the  same.  We  will  now  outline  the  key  computational  requirements  for  the  basic  motion 
planning  algorithm  and  the  two  tracking  control  algorithms  to  help  characterize  the 
solution.  For  this  discussion,  we  will  use  the  variable  m  to  denote  the  number  of  states  in 
the  configuration  vector  and  the  variable  n  to  denote  the  number  of  time  steps  between 
to  and  tf  in  a  simulation.  We  will  also  use  the  order  notation  O(-)  to  characterize  the 
rate  at  which  the  processing  time  grows  with  respect  to  the  number  of  states  and  the 
number  of  time  steps.  We  will  present  a  general  estimate  of  the  processing  growth  rate 
and  will  not  attempt  an  in-depth  investigation  to  this  aspect  of  the  simulations. 

As  described  in  Section  3.2,  the  basic  motion  planning  algorithm  consists  of  four 
main  steps.  The  first  step  generates  the  polynomial  curve  in  the  x-y  plane  that  connects 
the  initial  and  final  positions  with  a  curve  having  the  correct  orientation  at  each  end¬ 
point.  This  operation  depends  on  only  the  initial  and  final  configurations  and  the  time 
interval,  but  not  the  number  of  steps  in  the  time  interval.  It  does  not  matter  if  we  use 
the  cubic  spline  or  the  Pythagorean  hodograph  curve  solution  for  the  first  step,  since 
both  approaches  are  independent  of  the  variable  n.  Increasing  the  number  of  states  in 
the  configuration  will  increase  the  processing  time,  and  the  algorithm  will  have  to  be 
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adapted  to  plan  position  curves  for  nonplanar  systems.  With  these  factors  in  mind,  we 
estimate  the  processing  time  for  the  first  step  as  0(m).  The  second  motion  planning  step 
estimates  the  complete  trajectory  and  the  corresponding  control  inputs  at  each  time  step 
n,  so  this  step  is  order  0(mn).  The  third  step  uses  the  estimated  inputs  in  conjunction 
with  the  differential  equations  of  motion  to  generate  a  feasible  trajectory  for  the  vehi¬ 
cle,  which  requires  0(mn )  operations.  The  final  motion  planning  step  implements  the 
iterative  .ff  “-filter.  Each  iteration  of  the  filter  requires  solving  a  differential  equation  to 
estimate  each  state  and  a  matrix  differential  equation  to  find  the  filter  covariance  ma¬ 
trix.  Both  differential  equations  are  evaluated  at  each  time  step,  so  the  fourth  step  is 
0{m2n).  Combining  the  four  steps  in  the  motion  planning  algorithm,  we  conclude  that 
the  processing  time  is  0(m2n). 

The  perfect  state  feedback  tracking  control  algorithm  contains  two  key  steps  to  de¬ 
velop  the  control  law.  The  first  step  is  to  linearize  the  equations  of  motion  at  each  time 
step  along  the  desired  trajectory.  This  operation  is  0(mn )  because  each  state  contributes 
one  linearization  equation  and  the  equations  are  evaluated  at  each  time  step.  The  sec¬ 
ond  step  in  the  algorithm  is  to  solve  the  generalized  Riccati  differential  equation  at  each 
time  step,  which  adds  0(m2n)  operations  to  the  processing  time.  The  m2  term  appears 
because  the  GRDE  is  a  matrix  differential  equation.  Overall,  the  perfect  state  feedback 
tracking  control  algorithm  is  0(m2n). 

The  analysis  of  the  computational  complexity  for  the  imperfect  state  measurement 
tracking  controller  includes  the  operations  required  for  the  perfect  state  feedback  case. 
We  must  also  account  for  the  additional  processing  to  estimate  the  complete  state  vector. 
To  estimate  the  state,  we  must  solve  a  vector  differential  equation  with  0(mn )  and  an 
associated  matrix  differential  equation  of  order  0(m2n).  Therefore,  the  imperfect  state 
measurement  tracking  controller  is  also  order  0(m2n). 
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If  we  couple  the  motion  planning  algorithm  with  either  of  the  tracking  controllers, 
the  computational  complexity  of  the  solution  grows  with  order  0(m2n).  This  result 
indicates  that  increasing  the  number  of  states  m  in  the  underactuated  system  increases 
the  processing  time  at  a  faster  rate  than  increasing  the  number  of  time  steps  n  in  the 
simulation.  As  indicated  above,  this  estimate  for  the  computational  complexity  does  not 
depend  on  the  type  of  software  used  to  implement  the  algorithms. 


6.8  Simulation  Results  Summary 

We  have  presented  in  this  chapter  a  series  of  simulations  that  illustrate  the  motion 
planning  and  controlling  algorithms  developed  in  Chapters  3,  4,  and  5.  The  simulation 
results  also  offer  empirical  support  for  Assumption  5.1  by  demonstrating  that  the  distur¬ 
bances  do  not  significantly  affect  the  terms  F(t)  and  /?(£),  which  appear  in  the  imperfect 
state  measurement  controller  design.  We  compared  our  design  techniques  to  other  ap¬ 
proaches  that  address  similar  problems  and  showed  how  our  design  approaches  work 
well  with  different  vehicle  models.  In  addition,  this  chapter  described  how  to  apply  the 
planning  and  controlling  techniques  to  allow  a  fully  actuated  vehicle  to  recover  from  an 
actuator  failure.  Finally,  we  outlined  the  computation  complexity  of  the  significant  algo¬ 
rithms  used  to  solve  the  motion  planning  and  tracking  control  problems.  The  simulation 
results  verify  that  our  design  techniques  provide  practical  solutions  to  the  challenging 
problem  of  planning  and  controlling  the  motion  of  a  nonlinear  underactuated  vehicle 
when  disturbances  are  present  and  only  imperfect  state  measurements  are  available  for 
feedback. 


125 


CHAPTER  7 


SUMMARY 


This  concluding  chapter  summarizes  the  contributions  of  the  completed  research  and 
outlines  the  additional  research  areas  related  to  this  effort  that  could  be  explored  in  the 
future. 


7.1  Completed  Research  and  Contributions 

Our  research  has  focused  on  developing  a  suite  of  tools  to  plan  and  robustly  control 
the  motion  of  a  nonlinear  underactuated  vehicle  when  disturbances  are  present  in  the 
system  and  only  imperfect  state  measurements  are  available.  Where  possible,  we  have 
used  //“-optimal  control  techniques  to  efficiently  construct  solutions  that  provide  ex¬ 
cellent  performance  under  a  variety  of  conditions.  There  are  five  main  contributions  of 
the  completed  research.  First,  we  showed  how  to  use  polynomial  curves  and  numerical 
methods  to  develop  feasible  trajectories  for  underactuated  vehicles.  This  approach  al¬ 
lowed  us  to  directly  develop  trajectories  without  extensive  search  routines.  The  second 
contribution  is  the  introduction  of  an  iterative  //“-filter  to  improve  the  planned  motions 
for  the  vehicle.  The  filter  accounted  for  the  underactuated  nature  of  the  system  to  help 
improve  the  results.  Third,  we  showed  how  to  combine  the  results  of  the  basic  motion 
planning  algorithm  with  existing  planning  techniques  to  generate  solutions  to  the  ob¬ 
stacle  avoidance  and  multiple  vehicle  planning  problems.  The  fourth  contribution  is  the 
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development  and  analysis  of  the  linearized  H°°  controller  for  an  underactuated  system. 
This  design  combined  two  standard  techniques  to  generate  a  control  law  with  excellent 
tracking  performance  under  demanding  conditions.  Finally,  this  research  constructed  an 
interlaced  //'“-filter  based  on  a  unique  decomposition  of  the  equations  for  the  underac¬ 
tuated  model.  This  approach  allowed  us  to  prove  disturbance  attenuation  results  for  the 
closed-loop  system  when  only  imperfect  state  measurements  were  available  for  feedback. 

Combined,  these  results  demonstrate  that  we  have  made  useful  advances  in  the  field 
of  underactuated  vehicle  motion  planning  and  control.  Along  the  way,  we  have  identified 
a  host  of  additional  research  opportunities  that  could  be  addressed  in  the  near  future. 

7.2  Potential  Research  Areas 

We  will  sketch  a  range  of  potential  research  areas  spawned  from  our  work  on  motion 
planning  and  control  for  underactuated  systems. 

There  are  several  ways  to  build  upon  our  motion  planning  results.  First,  our  results 
have  focused  on  the  planar  vehicle  case,  but  the  basic  motion  planning  algorithm  could 
be  adapted  to  handle  three-dimensional  scenarios  as  well.  The  key  challenges  with  this 
approach  would  include  finding  a  way  to  efficiently  generate  the  initial  polynomial  curve, 
as  well  as  using  the  equations  of  motion  for  the  system  to  derive  a  feasible  trajectory  from 
the  initial  curve.  Another  research  area  would  be  to  further  explore  the  rapidly-exploring 
random  trees  approach  to  solving  motion  planning  problems  for  underactuated  systems. 
The  current  approach  could  be  improved  by  refining  the  algorithm  and  through  more 
efficient  software  coding  to  help  reduce  the  processing  time.  A  third  motion  planning 
contribution  would  be  to  use  our  basic  planning  algorithm  to  investigate  the  decoupled 
approach  to  solving  the  multiple  vehicle  planning  problem.  The  decoupled  approach 
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offers  computational  advantages  over  the  centralized  method  we  employed  and  may  offer 
more  elegant  solutions  to  the  problem. 

The  list  of  potential  research  areas  related  to  the  controller  design  is  quite  extensive 
and  offers  some  significant  challenges.  Two  control  design  techniques  that  could  be 
applied  to  the  underactuated  vehicle  tracking  problem  are  pseudolinearization  and  gain 
scheduling.  Pseudolinearization  may  provide  a  reasonable  solution  to  the  problem,  but 
requires  developing  a  transformation  to  make  the  system  linearizable  about  the  desired 
trajectory.  A  gain  scheduling  approach  can  likely  solve  this  problem  as  well,  but  the  key 
elements  in  the  design  will  be  to  select  appropriate  operating  points  that  characterize 
different  types  of  motion  and  to  determine  how  to  schedule  among  the  operating  points. 
These  control  techniques  are  closely  related  and  it  may  be  efficient  to  conduct  a  combined 
study  of  the  two  approaches  for  underactuated  vehicles. 

In  Chapter  1,  we  mentioned  three  restrictions  on  our  research  to  help  limit  the  scope 
of  the  project.  Each  of  these  restrictions  points  to  a  new  research  direction  for  underac- 
tuated  vehicle  control  and  we  will  comment  on  two  of  three  areas.  For  realistic  vehicles, 
adaptive  control  techniques  are  very  practical  and  should  be  explored.  Along  with  adap¬ 
tive  control,  parameter  estimation  is  an  important  aspect  of  the  problem.  Both  of  these 
techniques  would  help  generate  controllers  that  could  respond  to  changing  conditions  in 
the  environment.  The  biologically  inspired  control  techniques  of  neural  networks,  fuzzy 
logic,  and  genetic  algorithms  also  offer  practical  methods  for  addressing  the  control  prob¬ 
lem.  These  approaches  are  especially  useful  when  the  underlying  system  is  nonlinear  and 
standard  approaches  do  not  yield  a  solution. 

Another  meaningful  area  of  research  would  be  to  consider  bounded  control  inputs 
or  the  effects  of  saturation  on  the  actuators.  The  model  used  for  our  results  did  not 
have  these  limitations,  but  they  represent  practical  situations  that  the  controller  should 
be  able  to  handle.  On  a  related  note,  extensions  to  our  research  could  also  account  for 
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actuator  dynamics.  We  assumed  that  the  actuators  could  instantaneously  achieve  the 
commanded  inputs.  A  more  realistic  model  would  account  for  the  transient  behavior  of 
the  actuators  when  developing  a  control  signal. 

The  final  two  suggested  research  topics  are  items  that  we  considered,  but  we  were 
not  able  to  develop  complete  solutions  for  them.  It  would  be  very  helpful  to  prove  a 
separation  principle  for  the  imperfect  state  measurement  problem  and  use  the  principle 
to  prove  local  exponential  stability  for  the  closed-loop  system.  This  line  of  investigation 
would  allow  us  to  describe  a  region  of  attraction  for  the  imperfect  state  measurement 
case  and  allow  system  designers  to  know  exactly  when  this  control  technique  would  be 
useful  for  a  particular  system.  The  other  area  that  is  worth  exploring  in  more  detail  is 
how  to  use  the  underactuated  controllers  to  recover  from  failures  with  a  fully  actuated 
system.  This  research  could  consider  the  complete  design  and  develop  experiments  to 
demonstrate  the  approach. 

This  outline  of  additional  research  areas  serves  as  a  first  step  to  extend  our  contribu¬ 
tions  on  planning  and  controlling  the  motion  of  an  underactuated  vehicle. 
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APPENDIX  A 


CUBIC  SPLINES  AND  HODOGRAPH  CURVES 

A.l  Introduction 

This  appendix  describes  two  methods  for  generating  polynomial  position  curves  for 
the  motion  planning  problem.  The  first  technique  uses  cubic  splines  and  the  second  ap¬ 
proach  uses  the  concept  of  Pythagorean  hodograph  curves.  We  will  use  both  methods 
to  solve  the  Hermite  interpolation  problem.  Cubic  splines  are  a  standard  technique  and 
Pythagorean  hodograph  curves  offer  an  alternative  method  for  generating  polynomials. 
Pythagorean  hodograph  curves  are  valuable  because  they  allow  for  an  analytical  com¬ 
putation  of  curvature,  length,  and  bending  energy  in  a  candidate  path.  We  can  also 
directly  calculate  offset  paths  located  a  fixed  distance  from  the  curve,  which  may  help  in 
analyzing  the  performance  of  the  motion  planning  algorithm.  We  solve  the  Pythagorean 
hodograph  curve  problem  using  polynomials  with  complex  numbers  to  provide  a  com¬ 
pact  representation.  We  used  Mathematica  to  perform  the  calculations  and  display  the 
results. 


A. 2  Problem  Formulation 

The  problem  we  would  like  to  solve  is  to  find  a  planar  x-y  position  path  that  connects 
two  points  and  satisfies  tangent  conditions  at  each  point.  This  problem  is  known  as  the 
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Hermite  interpolation  problem  [99]  and  has  a  well-known  solution  if  the  path  coordinates 
are  characterized  by  cubic  polynomials.  This  section  will  present  the  notation  used  to 
describe  the  problem.  Section  A. 3  will  provide  the  solution  using  cubic  splines  and 
Sections  A. 4  and  A. 5  will  describe  the  second  solution  to  the  problem  using  Pythagorean 
hodograph  curves.  Section  A.6  will  provide  a  few  examples  to  illustrate  the  Pythagorean 
hodograph  technique  for  solving  the  Hermite  interpolation  problem. 

Our  objective  is  to  find  polynomials  x(t )  and  y(t)  such  that  the  path  generated  by 
{x(t),y(t)}  with  t  e  [0, 1]  satisfies  a  set  of  boundary  conditions.  The  time  interval  has 
been  normalized  to  [0,1]  without  loss  of  generality.  We  define  r(t)  :=  {x(t),y(t)}  and 
r '(t)  :=  {x'(t),  y'(t)}  to  simplify  the  notation  for  the  parametric  curve  and  its  derivative. 
We  are  given  the  values  of  r(0),  r(l),  r'(0),  and  r'(l)  as  the  boundary  conditions  for  the 
problem.  We  note  that  we  can  easily  compute  these  values  from  the  information  available 
to  solve  the  motion  planning  problem  presented  in  Chapter  3. 

A. 3  Cubic  Spline  Solution 

The  solution  for  the  cubic  spline  interpolation  problem  solves  a  set  of  simultaneous 
equations  based  on  the  polynomials  for  each  coordinate.  We  have 

x(t)  —  axt 3  +  bxt 2  +  cxt  +  dx  (A.l) 

y(t)  =  CLyt 3  +  byt2  +  Cyt  +  dy  (A. 2) 

and 

x'(t)  =  3  axt2  +  2  bxt  +  cx  (A. 3) 

y'(t)  =  3ayt 2  +  2byt  +  cy.  (A -4) 

Consider  the  x(t)  coordinate  first.  If  we  evaluate  x(t)  and  x'(t)  each  at  t  =  0  and  t  =  1, 
we  have  four  equations.  There  are  four  unknown  parameters  ( ax,bx,cx,dx ),  so  we  can 
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solve  for  the  parameters  uniquely.  In  terms  of  the  boundary  conditions,  the  solution  is 


ax  =  2[x(0)  -  x(l)]  +  rr'(O)  +  a/(l) 
bx  =  3[:r(l)  -  x(0)]  -  2a;'(0)  -  x'(l) 
cx  =  rr'(O) 
dx  -  a;(0). 

We  can  construct  a  similar  solution  for  the  y  coordinate. 

In  practice,  we  generate  the  cubic  spline  using  a  single  Matlab  command,  spline  .m, 
which  returns  the  polynomials  for  the  x  and  y  coordinates.  The  Matlab  routine  uses 
the  method  of  divided  difference  to  find  the  polynomials  that  represent  the  spline.  The 
Matlab  documentation  and  de  Boor’s  text  on  splines  [100]  provide  more  detailed  infor¬ 
mation  about  the  approach.  The  Matlab  function  spline. m  is  equivalent  to  solving  a 
system  of  four  linear  equations  described  above  for  each  coordinate. 

The  solution  presented  above  using  cubic  splines  readily  generates  a  candidate  path, 
but  it  may  be  difficult  to  find  analytical  expressions  for  curvature  along  the  path  or  path 
length.  We  can  numerically  approximate  these  values  for  specific  problems,  but  it  is 
difficult  to  find  general  solutions.  Pythagorean  hodograph  curves  offer  an  alternative  ap¬ 
proach  that  solves  the  Hermite  interpolation  problem  and  provides  analytical  expressions 
for  curvature  and  path  length. 

A. 4  Pythagorean  Hodograph  Curves 

Pythagorean  hodograph  curves  were  introduced  by  Farouki  and  Sakkalis  [101]  as  a 
mathematical  concept  that  could  assist  with  computer  modeling  and  graphics.  Farouki 
and  Neff  [102]  used  Pythagorean  hodograph  curves  to  solve  the  Hermite  interpolation 
problem,  and  Farouki  [103]  further  developed  the  concept  to  compute  the  bending  energy 
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of  the  curves.  Bruyninckx  and  Reynaerts  [104]  applied  Pythagorean  hodograph  curves 
to  path  planning  for  mobile  and  hyper-redundant  robots,  which  is  closely  related  to  the 
path  planning  problem.  This  section  will  explain  the  general  concept  of  Pythagorean 
hodograph  curves  and  describe  how  some  of  their  properties  are  particularly  useful  for 
path  planning. 

The  hodograph  of  the  planar  curve  r(t)  —  {x(t),y(t)}  is  the  curve  of  velocities  given 
by  r '(t)  :=  {x' (t) ,  y' (t)} .  A  planar  curve  is  a  Pythagorean  hodograph  (PH)  curve  if  its 
hodograph  satisfies  the  algebraic  Pythagorean  constraint 

x '2  ( t )  +  y'2  (t)  =  a2  ( t )  ( A .  5) 

for  some  polynomial  a(t).  To  solve  for  the  Pythagorean  hodograph  curves  we  will  express 
the  curves  as  complex- valued  polynomials  of  the  form  x(t)  +  iy(t).  This  approach  will 
allow  us  to  develop  a  more  compact  solution  and  to  exploit  the  complex  algebra  capabil¬ 
ities  of  standard  computational  software.  Before  we  present  the  solution  using  complex 
notation,  we  consider  some  facts  about  PH  curves  using  real  polynomials. 

Following  [102],  we  let  u(t),  v(t),  and  w(t)  be  nonzero  real  polynomials  such  that  u(t) 
and  v(t)  are  relatively  prime  and  not  both  constants.  If  we  form  a  parametric  curve  with 
the  derivatives 

x'(t)  =  w(t)[u2(t)  -  v2(t)]  and  y'(t )  =  2 w(t)u(t)v(t)  (A.6) 

then  the  curve  will  be  a  Pythagorean  hodograph  curve  with 

a(t)  =  u;(t)[u2(t)+u2(t)].  (A. 7) 

We  will  use  this  type  of  construction  with  the  complex  polynomials  when  we  present  the 
detailed  solution. 

Pythagorean  hodograph  curves  are  different  from  other  polynomial  curves  because  we 
can  express  the  parametric  speed,  unit  tangent,  unit  normal,  and  curvature  as  rational 
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functions  of  parameter  t.  The  parametric  speed  is  given  by 


^  =  y/x,2(t)  +  y,2(t)  =  \r'(t)\  =  a  {t). 

Having  a  polynomial  expression  for  the  speed  will  make  it  easy  to  compute  the  length 
of  each  curve  without  numerical  approximations.  We  can  express  the  unit  tangent  t, 
unit  normal  n,  and  curvature  k  in  terms  of  the  polynomials  u,  v,  and  w  (assuming  w  is 
nonnegative)  as 

(u2  —  v2, 2 uv)  (2 uv,  v2  —  u2)  2 (uv1  —  u'v)  .  . 

1  9  i  9  5  ^  2  i  2  ’  ^  /  o  i  2\2  *  v^’^) 

U 1  +  V2  U2  +  V2  w(u 2  +  V2)2 

We  can  use  the  unit  normal  to  find  offset  curves  located  at  a  signed  distance  d  from  the 
original  curve  using 

rd(t)  =  r  (t)  +  dn(t).  (A.9) 


These  offset  curves  will  help  us  describe  a  region  around  a  desired  path  that  the  vehicle 
must  stay  within  to  meet  performance  objectives.  Again,  having  a  rational  polynomial 
expression  will  simplify  the  calculations. 

The  lowest-order  Pythagorean  hodograph  curves  that  satisfy  the  Hermite  interpola¬ 
tion  constraints  are  the  quintic  (fifth-order)  splines.  The  quintics  do  not  give  a  unique 
solution  and  there  are  always  four  curves  that  satisfy  the  constraints.  Of  the  four  curves, 
there  is  one  that  gives  a  reasonable  shape  without  too  much  turning  or  bending  [102]. 
We  would  like  to  find  all  of  the  PH  curves  that  satisfy  a  set  of  boundary  conditions  and 
then  automatically  select  the  one  that  has  the  most  appropriate  shape. 

Using  another  standard  representation,  we  can  express  the  curve  r(t)  in  Bernstein- 


Bezier  spline  form: 


/ 


fc=0 


n 


\k  j 


(1  -t)n~ktk,  <€[0,1]. 


(A.10) 
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We  can  solve  for  the  first  two  and  last  two  control  points  Pk  using  the  boundary  conditions 
for  the  curve  given  by  r(0),  r(l),  r'(0),  and  r'(l).  The  resulting  expressions  are 

Po  =  r(0),  pi  =  r(0)  +  -r'(0), 
pn_i  =  r(l)  -  ^r'(l),  pn  =  r(l). 

For  quintic  PH  curves,  we  can  solve  for  control  points  Po,  Pi,  P4,  and  p5  directly.  Farouki 
and  Neff  [102]  present  a  procedure  for  finding  the  other  two  control  points  using  real 
polynomials,  but  we  will  not  discuss  it  here. 

This  section  has  presented  the  basic  concept  of  a  Pythagorean  hodograph  curve  and 
some  of  its  valuable  properties.  The  next  section  will  describe  how  to  find  the  four 
PH  curves  that  solve  the  Hermite  interpolation  problem  and  how  to  select  the  most 
reasonable  curve  for  the  path  planning  problem. 

A. 5  Pythagorean  Hodograph  Solution 

We  now  present  the  solution  to  the  Hermite  interpolation  problem  using  Pythagorean 
hodograph  curves  and  a  complex  number  representation.  As  mentioned  above,  the  solu¬ 
tion  can  be  stated  using  only  real  numbers,  but  the  complex  number  approach  allows  us  to 
take  advantage  of  some  of  the  computational  features  of  the  standard  computational  soft¬ 
ware  packages,  such  as  Matlab  or  Mathematica.  These  results  were  originally  reported 
by  Farouki  and  Neff  [102]  and  were  summarized  by  Bruyninckx  and  Reynaerts  [104].  We 
also  extract  some  of  the  results  from  Farouki  [103]  to  compute  the  length  and  energy  of 
the  curves. 

We  will  represent  the  parametric  curves  r(£)  as  a  complex- valued  polynomial  x(t)  + 
iy(t)  and  use  the  standard  algebraic  manipulations  for  complex  numbers.  The  results 
depend  on  a  key  lemma  from  Farouki  and  Neff  [102]: 
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Lemma  A.l  In  the  complex  representation,  the  regular  Pythagorean  holograph  curves 
correspond  to  those  curves  whose  hodographs  are  perfect  squares  of  complex  polynomials 
having  relatively  prime  real  and  imaginary  parts. 

Proof.  The  square  of  the  complex  polynomial  p(£)  =  u(t)  +  iv(t)  is  p2(t)  =  u2(t)  — 
v2(t)  +  i2u(t)v(t),  whose  real  and  imaginary  parts  are  seen  to  be  of  the  Pythagorean 
form  (A. 6)  with  w(t)  =  1.  Conversely,  any  complex  hodograph  having  real  and  imaginary 
parts  of  the  form  (A. 6)  with  w(t)  =  1  is  simply  the  square  of  the  complex  polynomial 
p (t)  =  u(t )  +  iv(t).  Moreover,  we  note  that  the  greatest  common  divisor  of  u  and  v, 
gcd(u,  v),  is  equal  to  one  if  and  only  if  gcd(u2  —  v2, 2 uv)  =  1.  □ 

Using  Lemma  A.l,  the  hodograph  of  any  regular  PH  quintic  curve  can  be  written  as 

r'(t)=k[(*-a)(t-b)]2  (A. 11) 

where  a,  b,  and  k  are  complex  numbers.  To  ensure  we  do  not  have  a  degenerate  solution, 
the  numbers  a  and  b  should  have  nonzero  imaginary  parts  and  they  should  not  be 
conjugates  of  each  other. 

The  first  step  in  the  solution  process  is  to  convert  the  boundary  conditions  to  standard 
form 

r(0)  =  0,  r'(0)  =  d0  and  r(l)  =  1,  r'(l)  =  di 

by  subtracting  r(0)  from  the  end  points  and  dividing  the  end  points  and  end  derivatives 
by  r(l)  —  r(0).  We  will  revert  back  to  normal  form  by  reversing  these  two  steps  at  the 
end. 

Next,  we  integrate  the  expression  (A. 11)  and  use  the  boundary  condition  r(0)  =  0  to 
determine  the  constant  of  integration  to  get 

r (t)  =  ^  [(t  -  a)5  -  5 (t  -  a )4(t  -  b)  +  10(t  -  a )3(t  -  b)2]  +  c  (A.12) 
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where  the  constant  is 


c  =  ^-(a5  -  5a4b  +  10a3b2).  (A. 13) 

We  have  now  reduced  the  problem  to  finding  the  complex  constants  a,  b,  and  k  such 
that  the  curve  defined  by  (A. 12)  and  (A. 13)  satisfies  r'(0)  =  d0,  r'(l)  =  di,  and  r(l)  =  1. 
The  solution  is  given  as  the  following  proposition  taken  from  [102]. 


Proposition  A.l  Let  p  be  either  of  the  two  complex  numbers  defined  by 


and  hence  let  a  be  either  of  the  two  solutions  to  the  quadratic  equation 


30 

of  —  3(1  -t-  p)ct  T  6 ff  -I-  2 p  -I-  6  —  - —  =  0. 

dx 


Then  if  pi  and  p%  are  the  two  roots  of 


(A.14) 


(A.15) 


p2  —  ap  +  p  =  0 


(A.16) 


the  values  of  &  and  b  in  (A.12)  are  given  by 


a  = 


Mi 


Mi  +  1 

The  corresponding  value  of  k  is  then 


and  b  = 


M2 


M2  +  1 


,  _  do  _  di 

a2b2  “  (1  —  a)2(l  —  b)2 

and  c  is  given  in  terms  of  a,  b,  and  k  by  expression  (A. 13). 


(A.17) 


(A.18) 


Farouki  and  Neff  [102]  present  a  brief  proof  for  this  proposition  which  consists  of 
verifying  that  the  results  satisfy  the  boundary  conditions  after  some  algebraic  manipula¬ 
tions.  The  two  solutions  from  (A.14)  and  the  two  solutions  from  (A.15)  combine  to  give 
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the  four  possible  values  for  a  and  lead  to  four  different  curves  that  satisfy  the  boundary- 
conditions. 


We  can  also  find  expressions  for  the  curve  length  and  curvature  given  as 


l  =  |k|2  |  |a|2|b|2  -  |a|2Re(b)  -  |b|2Re(a)  +  ^ ±...4Re(a^Re(b)  +  M  +  I 

3  5 


(A.19) 


and 


K(t)  = 


Im(rr) 

Irl3 


(A. 20) 


where 


r  =  k(t  —  a)2(£  —  b)2  (A.21) 

r  =  2k(t  —  a)(t  —  b)(2t  —  a  —  b).  (A. 22) 


Note  that  j  •  |  is  the  absolute  value  and  •  is  the  complex  conjugate  of  a  complex  number. 
The  bending  energy  of  the  curve  is  given  by 


E  = 


(A. 23) 


A  detailed  expression  for  the  total  bending  energy  in  terms  of  the  parameters  a,  b,  and 
k  is  given  by  Farouki  [103,  p.  234-235]  and  is  not  repeated  here.  To  select  a  single  curve 
from  the  four  available,  we  will  choose  the  PH  curve  with  the  minimum  total  bending 
energy  as  the  candidate  curve  for  the  path  planning  process.  Farouki  and  Neff  [102] 
propose  using  the  curve  with  the  smallest  absolute  rotation  index  given  by 

Rabs  =  ^  J  M  ds  (A. 24) 

as  an  alternative  criterion  to  bending  energy.  The  two  criteria  will  often  give  the  same 
results.  The  bending  energy  criterion  is  more  appropriate  for  the  path  planning  problem 
because  it  will  avoid  regions  of  high  curvature.  The  rotation  index  criterion  avoids 
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solutions  that  loop,  but  allows  higher  curvatures.  We  consider  making  a  low  curvature 
loop  a  more  acceptable  path  than  one  with  less  overall  turning,  but  with  regions  of  higher 
curvature. 

Bruyninckx  and  Reynaerts  [104]  present  a  more  general  performance  index  given  by 

f  /c2(s)  ds  +  'y  f  ds  (A. 25) 

</o  Jo 

where  7  is  a  weighting  factor  between  bending  energy  and  arc  length.  We  have  all  of  the 
tools  in  place  to  calculate  this  performance  index,  but  did  not  use  it  at  this  time. 

This  section  has  described  how  to  compute  the  Pythagorean  hodograph  curves  that 
solve  the  Hermite  interpolation  problem  using  a  complex  polynomial  representation.  We 
also  explained  how  to  use  the  total  bending  energy  as  a  selection  criterion  to  pick  an 
appropriate  curve  for  path  planning  from  the  four  available  solutions.  The  next  section 
will  illustrate  the  approach  with  a  few  simple  examples. 


A. 6  Example  Pythagorean  Hodograph  Curves 

We  used  Mathematica  to  calculate  the  solution  to  the  Hermite  interpolation  problem 
for  several  examples.  Bruyninckx  and  Reynaerts  used  Matlab  to  generate  the  examples 
shown  in  [104]  and  generously  made  their  code  available  to  us.  We  implemented  their 
Matlab  code  and  made  minor  improvements  in  it  to  replicate  some  of  the  results  in  [104]. 
The  Matlab  code  does  not  explicitly  solve  for  the  quintic  polynomials  for  x(t)  and  y(t) 
and  instead  relies  on  numerical  evaluation  to  generate  the  curves.  We  corrected  this 
deficiency  by  implementing  the  algorithm  in  Mathematica  and  using  the  software  to 
further  improve  the  results.  All  of  the  simulations  presented  here  were  completed  using 
Mathematica  and  the  results  verified  with  the  original  Matlab  code. 
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Figure  A.l  is  replica  of  Figure  3  in  [104]  and  shows  the  four  PH  curves  satisfying 
the  conditions  r(0)  =  {0,0},  r(l)  =  {1, 1},  r'(0)  =  3  x  {1,0},  and  r'(l)  =  2  x  {-1, 1}. 
Figure  A. 2  shows  the  corresponding  curvature  profiles  for  these  four  curves,  and  it  is  fairly 
obvious  which  one  has  the  lowest  bending  energy  (i.e.,  the  lower-left  plot).  Figure  A.3 
shows  the  PH  curve  with  the  lowest  bending  energy  along  with  two  offset  curves  located 
a  distance  d  =  0.41  units  from  the  original  curve.  This  offset  distance  represents  the 
maximum  value  for  a  smooth  offset  curve.  Choosing  a  larger  offset  distance  would  create 
a  boundary  curve  that  crosses  over  itself  because  of  the  shape  of  the  original  curve. 


y 


Figure  A.l  Four  example  Pythagorean  hodograph  curves.  The  curves  satisfy  the  bound¬ 
ary  conditions  r(0)  =  {0,  0},  r(l)  =  {1, 1},  d0  =  3  x  {1,  0},  and  d*  =  2  x  {—1, 1}.  Only 
one  curve  represents  a  reasonable  candidate  path  for  motion  planning. 

We  present  two  more  figures  that  illustrate  the  types  of  solutions  available  with 
Pythagorean  hodograph  curves.  Figure  A.4  shows  the  four  PH  curves  for  r(0)  =  {0, 0}, 
r(l)  =  {0,4},  r'(0)  =  3  x  {1,0},  and  r'(l)  =  2  x  {1,0}.  Figure  A.5  shows  the  four 
PH  curves  for  r(0)  =  {0,0},  r(l)  =  {1,1},  r'(0)  =  2  x  {1,0},  and  r'(l)  =  2x  {1,0}. 
In  all  three  examples,  the  curve  with  the  minimum  bending  energy  represents  the  most 
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reasonable  path  to  use  as  the  starting  point  for  planning  the  motion  of  a  vehicle.  It  is 
very  easy  to  identify  the  appropriate  curve  and,  since  it  is  a  polynomial  position  curve, 
there  is  no  difference  between  the  Pythagorean  hodograph  curve  and  the  cubic  spline  for 
the  remainder  of  the  motion  planning  algorithm. 
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Figure  A. 2  Curvature  profiles  for  the  four  Pythagorean  hodograph  curves.  Notice  the 
large  variation  in  the  magnitude  of  the  curvatures.  The  lower  left  plot  corresponds  to 
the  minimum  energy  curve. 
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Figure  A. 3  Minimum  energy  curve  with  offset  boundaries.  The  energy  of  this  curve  is 
3.99  units  and  the  length  is  2.01  units.  The  offset  curves  are  located  0.41  units  away 
from  the  original  curve.  The  maximum  curvature  for  this  curve  is  k  =  2.42  and  we 
placed  the  offset  curves  at  a  distance  equal  to  the  minimum  radius  of  curvature  p  =  1/k. 
Constructing  offset  curves  at  a  distance  more  than  pmjn  causes  a  distorted  offset  boundary. 
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Figure  A. 4  Four  example  Pythagorean  hodograph  curves.  The  curves  satisfy  the  bound¬ 
ary  conditions  r(0)  =  {0, 0},  r(l)  =  {0, 4},  d0  =  3  x  {1, 0},  and  di  =  2  x  {1, 0}. 
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Figure  A. 5  Four  example  Pythagorean  hodograph  curves.  The  curves  satisfy  the  bound¬ 
ary  conditions  r(0)  =  {0, 0},  r(l)  =  {1, 1},  do  =  2  x  {1, 0},  and  di  =  2  x  {1, 0}. 
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A.  7  Summary 


This  appendix  presented  two  techniques  for  solving  the  Hermite  interpolation  prob¬ 
lem.  The  first  technique  is  relatively  common  and  relies  on  cubic  splines  to  find  the  so¬ 
lution.  The  second  approach  uses  the  concept  of  Pythagorean  hodograph  curves,  which 
have  some  attractive  properties  for  motion  planning  problems.  We  also  illustrated  three 
examples  of  the  Pythagorean  hodograph  curve  approach. 
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Abstract:  This  thesis  presents  new  techniques  for  planning  and  robustly  controlling  the 
motion  of  nonlinear  underactuated  vehicles  when  disturbances  are  present  and  only  imper¬ 
fect  state  measurements  are  available  for  feedback.  The  basic  motion  planning  algorithm 
uses  cubic  splines  or  Pythagorean  hodograph  curves  to  connect  the  initial  and  final  configu¬ 
rations  and  to  generate  a  feasible  trajectory  for  the  system.  The  feasible  trajectory  and  its 
control  inputs  are  improved  through  an  iterative  #°°-filter.  Techniques  are  demonstrated  for 
generalizing  the  motion  planning  algorithm  to  address  the  obstacle  avoidance,  multiple  ve¬ 
hicle,  and  minimum  distance  planning  problems.  To  track  a  desired  trajectory,  first  a  state 
feedback  control  law  is  developed  for  the  linearized  system  using  an  /f°°-optimal  design. 
The  state  feedback  controller  produces  a  locally  exponentially  stable  closed-loop  system  and 
guarantees  a  precomputable  level  of  disturbance  attenuation  for  the  system.  Subsequently, 
an  imperfect  state  measurement  feedback  controller  is  developed  by  combining  a  state  esti¬ 
mate  with  the  state  feedback  control  law.  The  state  estimator  exploits  a  unique  structure  in 
the  nonlinear  equations  of  motion  to  decompose  the  system  into  two  interlaced  subsystems, 
which  leads  to  a  direct  solution.  The  estimator  is  actually  an  #°°-filter,  and  under  it  the 
controller  achieves  a  modified  form  of  disturbance  attenuation.  Simulations  included  in  the 
thesis  illustrate  the  motion  planning,  the  state  feedback  control,  and  the  imperfect  state 
measurement  control  algorithms  derived,  for  selected  models  of  nonlinear  underactuated 
vehicles. 
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